#define L_ARM

using System;
using System.Data.SqlTypes;
using System.Collections.Generic;
using System.Collections;
//http://www.gamedev.ru/code/articles/?id=4215&page=2
namespace geometry
{
    public static class Gm
    {
        public enum DecartPosition
        {
            Origin = 0,
            At1 = 1,
            At2 = 2,
            At3 = 3,
            At4 = 4,
            AtX_PlusY = 5,
            AtX_MinusY = 6,
            AtY_PlusX = 7,
            AtY_MinusX = 8
        }

        #region Common

        /// <summary>
        /// Calculation accuracy
        /// </summary>
        public static double Accuracy { get { return 0.0000001; } }

        /// <summary>
        /// Max digits amount
        /// </summary>
        public static uint MaxDigits { get { return 7; } }

        /// <summary>
        /// Are numbers approximately equals?
        /// </summary>
        /// <param name="d1">First number</param>
        /// <param name="d2">Second number</param>
        /// <param name="tol">Percision</param>
        /// <returns>True - if diference between numbers less than percision. Else - return False.</returns>
        public static bool ApproximatelyEqual(double d1, double d2, double tol)
        {
            return (Math.Abs(d1 - d2) < tol);
        }

        /// <summary>
        /// Are numbers approximately equals?
        /// </summary>
        /// <param name="d1">First number</param>
        /// <param name="d2">Second number</param>
        /// <returns>True - if diference between numbers less than calculation accuracy (0.0000001). Else - return False.</returns>
        public static bool ApproximatelyEqual(double d1, double d2)
        {
            return (Math.Abs(d1 - d2) < Accuracy);
        }

        /// <summary>
        /// Are numbers approximately equals?
        /// </summary>
        /// <param name="d1">First number</param>
        /// <param name="d2">Second number</param>
        /// <param name="tol">Percision</param>
        /// <returns>True - if diference between numbers less than percision. Else - return False.</returns>
        public static bool ApproximatelyEqual(float d1, float d2, float tol)
        {
            return (Math.Abs(d1 - d2) < tol);
        }

        /// <summary>
        /// Are numbers approximately equals?
        /// </summary>
        /// <param name="d1">First number</param>
        /// <param name="d2">Second number</param>
        /// <returns>True - if diference between numbers less than calculation accuracy (0.0000001). Else - return False.</returns>
        public static bool ApproximatelyEqual(float d1, float d2)
        {
            return (Math.Abs(d1 - d2) < Accuracy);
        }

        /// <summary>
        /// Determines whether the number needed to be rounded
        /// </summary>
        /// <param name="digits">Digits amount</param>
        /// <param name="factor1">Determine number percision</param>
        /// <param name="factor2">Determine number capacity</param>
        /// <returns>True - if amount of digits equals to 0, else - False.</returns>
        public static bool Factor(uint digits, out double factor1, out double factor2) 
        {
            factor1 = 1;
            factor2 = 1;
            
            if (digits > MaxDigits)
                throw new ArgumentException("Argument exception: digits");

            switch (digits)
            {
                case 0:
                    factor1 = factor2 = 0;
                    return true;
                case 1:
                    factor1 = 0.1;
                    factor2 = 10;
                    break;
                case 2:
                    factor1 = 0.01;
                    factor2 = 100;
                    break;
                case 3:
                    factor1 = 0.001;
                    factor2 = 1000;
                    break;
                case 4:
                    factor1 = 0.0001;
                    factor2 = 10000;
                    break;
                case 5:
                    factor1 = 0.00001;
                    factor2 = 100000;
                    break;
                default:
                    for (int i = 0; i < digits; i++)
                    {
                        factor1 *= 0.1;
                        factor2 *= 10;
                    }
                    break;
            }
            return false;
        }

        /// <summary>
        /// Determines whether the number needed to be rounded
        /// </summary>
        /// <param name="digits">Digits amount</param>
        /// <param name="factor1">Determine number percision</param>
        /// <param name="factor2">Determine number capacity</param>
        /// <returns>True - if amount of digits equals to 0, else - False.</returns>
        public static bool Factor(uint digits, out float factor1, out float factor2)
        {
            factor1 = 1;
            factor2 = 1;

            if (digits > MaxDigits)
                throw new ArgumentException("Argument exception: digits");

            switch (digits)
            {
                case 0:
                    factor1 = factor2 = 0f;
                    return true;
                case 1:
                    factor1 = 0.1f;
                    factor2 = 10f;
                    break;
                case 2:
                    factor1 = 0.01f;
                    factor2 = 100f;
                    break;
                case 3:
                    factor1 = 0.001f;
                    factor2 = 1000f;
                    break;
                case 4:
                    factor1 = 0.0001f;
                    factor2 = 10000f;
                    break;
                case 5:
                    factor1 = 0.00001f;
                    factor2 = 100000f;
                    break;
                default:
                    for (int i = 0; i < digits; i++)
                    {
                        factor1 *= 0.1f;
                        factor2 *= 10f;
                    }
                    break;
            }
            return false;
        }

        /// <summary>
        /// Converts degree to radian
        /// </summary>
        /// <param name="angle">Angle value in degrees</param>
        /// <returns>Angle value in radians</returns>
        public static double DegToRad(float angle) { return angle * Math.PI / 180; }

        /// <summary>
        /// Converts degree to radian
        /// </summary>
        /// <param name="angle">Angle value in degrees</param>
        /// <returns>Angle value in radians</returns>
        public static double DegToRad(double angle) { return angle * Math.PI / 180; }

        /// <summary>
        /// Converts radian to degree
        /// </summary>
        /// <param name="angle">Angle value in radians</param>
        /// <returns>Angle value in degrees</returns>
        public static double RadToDeg(float angle) { return angle * 180 / Math.PI; }

        /// <summary>
        /// Convert radian to degree
        /// </summary>
        /// <param name="angle">Angle value in radians</param>
        /// <returns>Angle value in degrees</returns>
        public static double RadToDeg(double angle) { return angle * 180/ Math.PI; }

        /// <summary>
        /// Rounds value to n symbols after separator
        /// </summary>
        /// <param name="value">Number value</param>
        /// <param name="digits">Amount of digits after separator</param>
        /// <returns>Rounded value</returns>
        public static double RoundTo(float value, uint digits)
        {
            float factor1 = default(float);
            float factor2 = factor1;
            float ret;
            Factor(digits, out factor1, out factor2);
            if (Factor(digits, out factor1, out factor2))
                ret = (int)value;
            else
                ret = ((int)(value * factor2)) * factor1;

            return ret;
        }

        /// <summary>
        /// Rounds value to n symbols after separator
        /// </summary>
        /// <param name="value">Number value</param>
        /// <param name="digits">Amount of digits after separator</param>
        /// <returns>Rounded value</returns>
        public static double RoundTo(double value, uint digits)
        {
            double factor1 = default(double);
            double factor2 = factor1;
            double ret;
            Factor(digits, out factor1, out factor2);
            if (Factor(digits, out factor1, out factor2))
                ret = (int)value;
            else
                ret = ((int)(value * factor2)) * factor1;

            return ret;
        }

        /// <summary>
        /// Rounds vector coordinates to n symbols after separator
        /// </summary>
        /// <param name="value">Vector value</param>
        /// <param name="digits">Amount of digits after separator</param>
        /// <returns>Rounded vector</returns>
        public static Vector RoundTo(Vector value, uint digits)
        {
            double factor1 = default(double);
            double factor2 = factor1;
            Vector ret = new Vector();
            if (Factor(digits, out factor1, out factor2))
            {
                ret.x = (int)value.x;
                ret.y = (int)value.y;
                ret.z = (int)value.z;
            }
            else
            {
                ret.x = ((int)(value.x * factor2)) * factor1;
                ret.y = ((int)(value.y * factor2)) * factor1;
                ret.z = ((int)(value.z * factor2)) * factor1;
            }
            return ret;
        }

        /// <summary>
        /// Rounds point coordinates to n symbols after separator
        /// </summary>
        /// <param name="value">Point value (3D)</param>
        /// <param name="digits">Amount of digits after separator</param>
        /// <returns>Rounded point</returns>
        public static Point RoundTo(Point value, uint digits)
        {
            double factor1 = default(double);
            double factor2 = factor1;
            Point ret = new Point();
            Factor(digits, out factor1, out factor2);
            if (Factor(digits, out factor1, out factor2))
            {
                ret.x = (int)value.x;
                ret.y = (int)value.y;
                ret.z = (int)value.z;
            }
            else
            {
                ret.x = ((int)(value.x * factor2)) * factor1;
                ret.y = ((int)(value.y * factor2)) * factor1;
                ret.z = ((int)(value.z * factor2)) * factor1;
            }
            return ret;
        }

        public static Quaterion RoundTo(Quaterion value, uint digits)
        {
            double factor1 = default(double);
            double factor2 = factor1;
            Quaterion ret = new Quaterion();
            Factor(digits, out factor1, out factor2);
            if (Factor(digits, out factor1, out factor2))
            {
                ret.Vector = new Vector((int)value.Vector.x, (int)value.Vector.y, (int)value.Vector.z);
                ret.W = (int)value.W;
            }
            else
            {
                ret.Vector = new Vector(((int)(value.Vector.x * factor2)) * factor1, ((int)(value.Vector.y * factor2)) * factor1, ((int)(value.Vector.z * factor2)) * factor1);
                ret.W = ((int)(value.W * factor2)) * factor1;
            }
            return ret;
        }

        public static Matrix3 RoundTo(Matrix3 value, uint digits)
        {
            double factor1 = default(double);
            double factor2 = factor1;
            double[] data = value.Data;

            Matrix3 ret = new Matrix3();
            Factor(digits, out factor1, out factor2);
            if (Factor(digits, out factor1, out factor2))
            {                
                for (int i = 0; i < data.Length; i++)
                    data[i] = (int)data[i];
            }
            else
            {
                for (int i = 0; i < data.Length; i++)
                    data[i] = ((int)(data[i] * factor2)) * factor1;                
            }
            return new Matrix3(data);
        }

        public static Transform RoundTo(Transform value, uint digits)
        {
            double factor1 = default(double);
            double factor2 = factor1;
            double[] data = value.Data;

            Transform ret = new Transform();
            Factor(digits, out factor1, out factor2);
            if (Factor(digits, out factor1, out factor2))
            {
                for (int i = 0; i < data.Length; i++)
                    data[i] = (int)data[i];
            }
            else
            {
                for (int i = 0; i < data.Length; i++)
                    data[i] = ((int)(data[i] * factor2)) * factor1;
            }
            return new Transform(data);
        }

        /// <summary>
        /// Split double array to vectors
        /// </summary>
        /// <param name="values"></param>
        /// <param name="vDim"></param>
        /// <param name="useRound"></param>
        /// <param name="digits"></param>
        /// <returns></returns>
        public static Vector[] SplitToVectors(double[] values, byte vDim, bool useRound, uint digits)
        {
            int vCCount = values.Length % vDim;

            if (vDim < 2 | vDim > 3)
                throw new ArgumentException("Gm.SplitToVectors: vDim must be 2 or 3");

            if (vCCount > 0 & vCCount < vDim - 1)
                throw new ArgumentException("Gm.SplitToVectors: values");

            Vector[] ret = new Vector[(int)(values.Length / vDim) + (vCCount > 0 ? 1 : 0)];

            for (int i = 0, counter = 0; i < values.Length & counter < ret.Length; i += vDim, counter++)
            {
                if (vDim > 1)
                {
                    ret[counter].x = values[i];
                    ret[counter].y = values[i + 1];
                }
                if (vDim > 2 && i + 2 < values.Length)
                    ret[counter].z = values[i + 2];

                // round
                if (useRound)
                    ret[counter].Round(digits);
            }
            return ret;
        }
        /// <summary>
        /// Split double array to points
        /// </summary>
        /// <param name="values"></param>
        /// <param name="vDim"></param>
        /// <param name="useRound"></param>
        /// <param name="digits"></param>
        /// <returns></returns>
        public static Point[] SplitToPoints(double[] values, byte vDim, bool useRound, uint digits)
        {
            int vCCount = values.Length % vDim;

            if (vDim < 2 | vDim > 4)
                throw new ArgumentException("Gm.SplitToPoints: vDim must be 2 or 3");

            if (vCCount > 0 & vCCount < vDim - 1)
                throw new ArgumentException("Gm.SplitToPoints: values");

            Point[] ret = new Point[(int)(values.Length / vDim) + (vCCount > 0 ? 1 : 0)];

            for (int i = 0, counter = 0; i < values.Length & counter < ret.Length; i += vDim, counter++)
            {
                if (vDim > 1)
                {
                    ret[counter].x = values[i];
                    ret[counter].y = values[i + 1];
                }
                if (vDim > 2 && i + 2 < values.Length)
                    ret[counter].z = values[i + 2];
                // uses round
                if (useRound)
                    ret[counter].Round(digits);
            }
            return ret;
        }

        #endregion

        #region Vector definition

        //[System.Diagnostics.DebuggerDisplay("{format(4)}", Name = "Vector")]
        /// <summary>
        /// Determine Vector structure
        /// </summary>
        [System.Runtime.InteropServices.StructLayout(System.Runtime.InteropServices.LayoutKind.Sequential)]
        public struct Vector : INullable, IEquatable<Vector>
        {
            public double x;
            public double y;
            public double z;
            private bool m_null;

            /// <summary>
            /// Get vector information (x;y;z),Length:value
            /// </summary>
            /// <param name="dig">Percision value</param>
            /// <returns>Vector information</returns>
            private string format(byte dig)
            {
                Vector v = RoundTo(this, dig);
                return "(" + v.x.ToString() + "; " + v.y.ToString() + "; " + v.z.ToString() + "), Length:" + v.Length().ToString();
            }

            /// <summary>
            /// Initialize new vector from three coordinate
            /// </summary>
            /// <param name="x">Coordinate X</param>
            /// <param name="y">Coordinate Y</param>
            /// <param name="z">Coordinate Z</param>
            public Vector(double x, double y, double z)
                : this()
            {
                this.x = x;
                this.y = y;
                this.z = z;
            }

            /// <summary>
            /// Initialize new vector from coordinate array
            /// </summary>
            /// <param name="points"> Coordinate array</param>
            public Vector(double[] points)
                : this()
            {
                if (points != null && points.Length >= 2)
                {
                    this.x = points[0];
                    this.y = points[1];
                    if (points.Length > 2)
                        this.z = points[2];
                }
                else
                    throw new ArgumentOutOfRangeException("Array size have to be equal or greater than 3 elements");
            }

            /// <summary>
            /// Initialize new vector from another vector
            /// </summary>
            /// <param name="v">Another vector</param>
            public Vector(Vector v)
                : this()
            {
                this.x = v.x;
                this.y = v.y;
                this.z = v.z;
            }

            /// <summary>
            /// Set vector coordinates
            /// </summary>
            /// <param name="x">Coordinate X</param>
            /// <param name="y">Coordinate Y</param>
            /// <param name="z">Coordinate Z</param>
            public void SetCoords(double x, double y, double z)
            {
                this.x = x;
                this.y = y;
                this.z = z;
            }

            /// <summary>
            /// Set vector coordinates
            /// </summary>
            /// <param name="v">Another vector</param>
            public void SetCoords(Vector v)
            {
                this.x = v.x;
                this.y = v.y;
                this.z = v.z;
            }

            /// <summary>
            /// Set vector coordinates
            /// </summary>
            /// <param name="points">Coordinates array</param>
            public void SetCoords(double[] points)
            {
                if (points != null && points.Length >= 2)
                {
                    this.x = points[0];
                    this.y = points[1];

                    if (points.Length >= 3)
                        this.z = points[2];
                }
                else
                    throw new ArgumentOutOfRangeException("Array size have to be equal or greater than 3 elements");
            }
           
            /// <summary>
            /// Current vector normalization (one-length vector)
            /// </summary>
            public void Normalize()
            {
                double dist = Length2();
                if (dist > 0)
                {
                    x /= dist;
                    y /= dist;
                    z /= dist;
                }
            }
            
            /// <summary>
            /// Vector normalization (one-length vector)
            /// </summary>
            /// <returns>Normilized vector</returns>
            public Vector GetNormalized()
            {
                Vector v = new Vector(x, y, z);
                double dist = v.Length();
                if (dist > 0)
                {
                    dist = 1 / dist;
                    v.x *= dist;
                    v.y *= dist;
                    v.z *= dist;
                }
                return v;
            }

            /// <summary>
            /// Creates parallel vector, at point
            /// </summary>
            /// <param name="point">Point value</param>
            /// <returns>Parallel vector</returns>
            public Vector GetParallel(Point point)
            {
                Vector v = this;
                v.x += point.x;
                v.y += point.y;
                v.z += point.z;
                return v;
            }

            /// <summary>
            /// Creates parallel vector at some distance from this
            /// </summary>
            /// <param name="distance">Distance</param>
            /// <returns>Parallel vector</returns>
            public Vector GetParallel(double distance)
            {
                Vector normal = GetPerpVector()*distance;
                Vector v = normal.GetPerpVector();
                if (this.GetNormalized() != v)
                    v.Inverce();
                return v;
            }

            /// <summary>
            /// Returns perpendicular vector
            /// </summary>
            /// <returns>Perpendicular vector</returns>
            public Vector GetPerpVector()
            {
                if ((AngleTo(new Vector(0, 1, 0)) < Accuracy) || (AngleTo(new Vector(0, -1, 0)) < Accuracy))
                {
                    return new Vector(y, 0, 0).GetNormalized();
                }
                return new Vector(z, 0, -x).GetNormalized();
            }

            /// <summary>
            /// Inverce current vector coordinates
            /// </summary>
            public void Inverce() { x = -x; y = -y; z = -z; }

            /// <summary>
            /// Returns opposite vector
            /// </summary>
            /// <returns>New vector opposite this</returns>
            public Vector GetInverse()
            {
                return new Vector(-x, -y, -z);
            }

            /// <summary>
            /// Calculate vector length
            /// </summary>
            /// <returns>Vector length</returns>
            public double Length()
            {
                return Math.Sqrt(x * x + y * y + z * z);
            }

            /// <summary>
            /// Calculate squared vector length
            /// </summary>
            /// <returns>Squared vector length</returns>
            public double Length2()
            {
                return x * x + y * y + z * z;
            }

            /// <summary>
            /// Rotate vector
            /// </summary>
            /// <param name="dir">Rotate direction vector</param>
            /// <param name="ang">Rotation angle</param>
            /// <returns>Rotated vector</returns>
            public Vector Rotate(Vector dir, double ang)
            {
                if (ang == 0)
                    return this;

                double c, s;
                double a;
                a = ang * Math.PI / 180;
                c = Math.Cos(a);
                s = Math.Sin(a);

                if (Double.IsNaN(c))
                    c = 0;
                if (Double.IsNaN(s))
                    s = 0;
                double nx, ny, nz;
                nx = x * (c + (1 - c) * (dir.x * dir.x)) + y * ((1 - c) * dir.y * dir.x + s * dir.z) + z * ((1 - c) * dir.z * dir.x - s * dir.y);
                ny = x * ((1 - c) * dir.y * dir.x - s * dir.z) + y * (c + (1 - c) * (dir.y * dir.y)) + z * ((1 - c) * dir.z * dir.y + s * dir.x);
                nz = x * ((1 - c) * dir.z * dir.x + s * dir.y) + y * ((1 - c) * dir.z * dir.y - s * dir.x) + z * (c + (1 - c) * (dir.z * dir.z));
                x = nx;
                y = ny;
                z = nz;
                return this;
            }           

            /// <summary>
            /// Round current vector coordinates
            /// </summary>
            /// <param name="digits">Amount of digits after separator</param>
            public void Round(uint digits)
            {
                double factor1 = default(double);
                double factor2 = factor1;                
                Factor(digits, out factor1, out factor2);
                if (Factor(digits, out factor1, out factor2))
                {
                    x = (int)x;
                    y = (int)y;
                    z = (int)z;
                }
                else
                {
                    x = ((int)(x * factor2)) * factor1;
                    y = ((int)(y * factor2)) * factor1;
                    z = ((int)(z * factor2)) * factor1;
                }               
            }

            /// <summary>
            /// Angle betwen current and another vectors
            /// </summary>
            /// <param name="vector">Another vector</param>
            /// <returns>Angle value (degrees)</returns>
            public double AngleTo(Vector vector)
            {              
                if (vector.Length() == 0)
                    return 0;

                double d = (this * vector) / (Length() * vector.Length());
                if (ApproximatelyEqual(d, 1, Accuracy))
                {
                    d = 1;
                }
                else if (ApproximatelyEqual(d, -1, Accuracy))
                {
                    d = -1;
                }
                return Math.Acos(d);
            }

            public double AngleToNonACos(Vector vector)
            {
                if (vector.Length() == 0)
                    return 0;

                double d = (this * vector) / (Length() * vector.Length());
                if (ApproximatelyEqual(d, 1, Accuracy))
                {
                    d = 1;
                }
                else if (ApproximatelyEqual(d, -1, Accuracy))
                {
                    d = -1;
                }
                return d;
            }

            public bool IsParallelTo(Vector vector)
            {
                double angle = AngleTo(vector);
                return (ApproximatelyEqual(angle, 0, Accuracy) || ApproximatelyEqual(angle, Math.PI, Accuracy));
            }

             /// <summary>
            /// Is current vector perpendicular to another one
            /// </summary>
            /// <param name="vector">Another vector</param>
            /// <returns>True - if vectors are perpendicular, else - False.</returns>
            public bool IsPerpendicularTo(Vector vector)
            {
                return (ApproximatelyEqual(AngleTo(vector), Math.PI / 2));
            }
            
            /// <summary>
            /// Return rotated vector at transform data
            /// </summary>
            /// <param name="transform">transform matrix</param>
            /// <returns>rotated vector</returns>
            public Vector this[Transform transform]
            {
                get { return transform.TransformVector(this); }
            }	

            #region INullable Members

            /// <summary>
            /// Is this a null vector? 
            /// </summary>
            public bool IsNull { get { return m_null; } }//ApproximatelyEqual(x, 0) && ApproximatelyEqual(y, 0) && ApproximatelyEqual(z, 0);

            #endregion

            #region IEquatable<Vector> Members

            /// <summary>
            /// Hash function for a certain type
            /// </summary>
            /// <returns>Vector Length</returns>
            public override int GetHashCode()
            {
                return (int)Math.Sqrt(x * x * y * y * z * z);
            }

            /// <summary>
            /// Indicates whether the current object equals to another object of the same type.
            /// </summary>
            /// <param name="o">anoter object</param>
            /// <returns>True - if current object equals to another object, else - False.</returns>
            public override bool Equals(object o)
            {
                if (o is Vector)
                {
                    Vector v = (Vector)o;
                    return !v.IsNull && Equals(v);
                }
                return false;
            }

            /// <summary>
            /// Indicates whether the current vector equals to another vector.
            /// </summary>
            /// <param name="v">Another vector</param>
            /// <returns>True - if current vector equals to another vector, else - False.</returns>
            public bool Equals(Vector v)
            {
                return this == v;
            }

            /// <summary>
            /// Indicates whether the current vector equals to another vector with some percision.
            /// </summary>
            /// <param name="vec">Another vector</param>
            /// <param name="tol">Percision value</param>
            /// <returns>True - if current vector equals to another vector with some percision, else - False.</returns>
            public bool Equals(Vector vec, int tol)
            {
                return
                    ApproximatelyEqual(vec.x, x, tol)
                  &&
                    ApproximatelyEqual(vec.y, y, tol)
                  &&
                    ApproximatelyEqual(vec.z, z, tol);
            }

            #endregion

            #region operators

            /// <summary>
            /// Adds two vectors
            /// </summary>
            /// <param name="v1">First vector</param>
            /// <param name="v2">Second vector</param>
            /// <returns>Vector, which is the sum of two other vector's coordinates.</returns>
            public static Vector operator +(Vector v1, Vector v2)
            {
                return new Vector(v1.x + v2.x, v1.y + v2.y, v1.z + v2.z);
            }

            /// <summary>
            /// Vectors difference
            /// </summary>
            /// <param name="v1">First vector</param>
            /// <param name="v2">Second vector</param>
            /// <returns>Vector, which is the difference of two other vector's coordinates.</returns>
            public static Vector operator -(Vector v1, Vector v2)
            {
                return new Vector(v1.x - v2.x, v1.y - v2.y, v1.z - v2.z);
            }

            /// <summary>
            /// Multiply vector on constant value
            /// </summary>
            /// <param name="v1">Vector</param>
            /// <param name="val">Constant value</param>
            /// <returns>Vector, which is the multiplication itself on value.</returns>
            public static Vector operator *(Vector v1, double val)
            {
                return new Vector(v1.x * val, v1.y * val, v1.z * val);
            }

            /// <summary>
            /// Divide vector on constant value
            /// </summary>
            /// <param name="v1">Vector</param>
            /// <param name="val">Constant value</param>
            /// <returns>Vector, which is the division itself on value.</returns>
            public static Vector operator /(Vector v1, double val)
            {
                return new Vector(v1.x / val, v1.y / val, v1.z / val);
            }

            /// <summary>
            /// Find vector multiplication of two vectors
            /// </summary>
            /// <param name="v1">First vector</param>
            /// <param name="v2">Second vector</param>
            /// <returns>Vector, which is the vector multiplication of two other vectors.</returns>
            public static Vector operator ^(Vector v1, Vector v2)
            {
#if L_ARM
                return new Vector(v2.y * v1.z - v2.z * v1.y, v2.z * v1.x - v2.x * v1.z, v2.x * v1.y - v2.y * v1.x);
#else
                return new Vector(v1.y * v2.z - v1.z * v2.y, v1.z * v2.x - v1.x * v2.z, v1.x * v2.y - v1.y * v2.x);
#endif
            }

            /// <summary>
            /// Find scalar multiplication of two vectors 
            /// </summary>
            /// <param name="v1">First vector</param>
            /// <param name="v2">Second vector</param>
            /// <returns>Number, which is the scalar multiplication of two other vectors.</returns>
            public static double operator *(Vector v1, Vector v2)
            {
                return v1.x * v2.x + v1.y * v2.y + v1.z * v2.z;
            }

            /// <summary>
            /// Are two vectors approximately equals each other?
            /// </summary>
            /// <param name="v1">First vector</param>
            /// <param name="v2">Second vector</param>
            /// <returns>True - if vectors are approximately equals each other, else - False.</returns>
            public static bool operator ==(Vector v1, Vector v2)
            {
                return
                    ApproximatelyEqual(v1.x, v2.x)
                  &&
                    ApproximatelyEqual(v1.y, v2.y)
                  &&
                    ApproximatelyEqual(v1.z, v2.z);
            }

            /// <summary>
            /// Inverse vector equals
            /// </summary>
            /// <param name="v1">First vector</param>
            /// <param name="v2">Second vector</param>
            /// <returns>True - if vectors are not equals each other, else - False.</returns>
            public static bool operator !=(Vector v1, Vector v2)
            {
                return !(v1 == v2);
            }

            /// <summary>
            /// Convert vector to array of coordinates
            /// </summary>
            /// <param name="v1">Vector value</param>
            /// <returns>Array of vector coordinates</returns>
            public static implicit operator double[](Vector v1)
            {
                double[] arr = new double[3];
                arr[0] = v1.x;
                arr[1] = v1.y;
                arr[2] = v1.z;
                return arr;
            }

            /// <summary>
            /// Convert array of coordinates to vector
            /// </summary>
            /// <param name="points">Array of vector coordinates</param>
            /// <returns>Vector value</returns>
            public static implicit operator Vector(double[] points)
            {
                Vector v = new Vector();
                if (points != null && points.Length >= 2)
                {
                    v.x = points[0];
                    v.y = points[1];
                    v.z = points[2];
                }
                else
                    throw new ArgumentOutOfRangeException("Array size have to be equal or greater than 3 elements");

                return v;
            }

            #endregion

            #region static members

            /// <summary>
            /// Create null vector
            /// </summary>
            public static Vector Null
            {
                get
                {
                    Vector v = new Vector();
                    v.m_null = true;
                    return v;
                }
            }

            /// <summary>
            /// Calculate angle between vectors v1 and v2. All vectors mut be vectors with unit length 
            /// </summary>
            /// <param name="axis">Required to define sign of the angle</param>
            /// <param name="v1">First vector</param>
            /// <param name="v2">Second vector</param>
            /// <returns>Return signed angle value in radians</returns>
            public static double GetAngleZBetweenVectors(Vector axis, Vector v1, Vector v2)
            {
                Vector tmp = (v1 ^ v2);
                double cosAngle = v2 * v1;
                double angle = Math.Acos(cosAngle);
                if (Double.IsNaN(angle))
                    angle = 0;
                if (tmp * axis < 0) //cos 90+ is negative
                    angle = -angle;
                return angle;
            }

            #endregion
        }

        #endregion

        #region Point definition
        /// <summary>
        /// Determine Point in 3D space
        /// </summary>
        //[System.Diagnostics.DebuggerDisplay("{format(4)}", Name = "Point")]
        [System.Runtime.InteropServices.StructLayout(System.Runtime.InteropServices.LayoutKind.Sequential)]
        public struct Point : IEquatable<Point>, INullable
        {
            /// <summary>
            /// Coordinates x,y,z of point
            /// </summary>
            public double x, y, z;

            /// <summary>
            /// Is this a null point with coordinates (0;0;0)?
            /// </summary>
            private bool m_null;

            /// <summary>
            /// Get point information (x;y;z).
            /// </summary>
            /// <param name="dig">Percision value</param>
            /// <returns>Point information.</returns>
            private string format(byte dig)
            {
                Point p = RoundTo(this, dig);
                return "(" + p.x.ToString() + "; " + p.y.ToString() + "; " + p.z.ToString() + ")";
            }

            /// <summary>
            /// Initialize new 3D point with coordinates
            /// </summary>
            /// <param name="x">Coordinate X</param>
            /// <param name="y">Coordinate Y</param>
            /// <param name="z">Coordinate Z</param>
            public Point(double x, double y, double z)
            {
                m_null = false;
                this.x = x;
                this.y = y;
                this.z = z;
            }

            /// <summary>
            /// Initialize new 2D point with coordinates
            /// </summary>
            /// <param name="x">Coordinate X</param>
            /// <param name="y">Coordinate Y</param>
            public Point(double x, double y) : this(x, y, 0) { }

            public void Translate(Vector vector)
            {
                x += vector.x;
                y += vector.y;
                z += vector.z;
            }

            public Point GetTranslated(Vector vector)
            {
                return this[vector];
            }

            public Point GetTransformed(Matrix3 matrix)
            {
                throw new NotImplementedException();
            }

            public void Transform(Matrix3 matrix)
            {
                throw new NotImplementedException();
            }

            /// <summary>
            /// Hash function for a certain type
            /// </summary>
            /// <returns>Hash code</returns>
            public override int GetHashCode()
            {
                return base.GetHashCode();
            }

            /// <summary>
            /// Round current point coordinates
            /// </summary>
            /// <param name="digits">Amount of digits after separator</param>
            public void Round(uint digits)
            {
                double factor1 = default(double);
                double factor2 = factor1;               
                if (Factor(digits, out factor1, out factor2))
                {
                    x = (int)x;
                    y = (int)y;
                    z = (int)z;
                }
                else
                {
                    x = ((int)(x * factor2)) * factor1;
                    y = ((int)(y * factor2)) * factor1;
                    z = ((int)(z * factor2)) * factor1;
                }
            }            

            /// <summary>
            /// Calculate opposite point coordinates
            /// </summary>
            public void MirrorAtOrigin()
            {
                x = -x;
                y = -y;
                z = -z;
            }           

            /// <summary>
            /// Canculate distance to another point
            /// </summary>
            /// <param name="point">Another point</param>
            /// <returns>Distance between points</returns>
            public double DistanceTo(Point point)
            {
                return this[point].Length();
            }

            #region IEquatable<Point> Members

            /// <summary>
            /// Indicates whether the current object equals to another object of the same type.
            /// </summary>
            /// <param name="o">anoter object</param>
            /// <returns>True - if current object equals to another object, else - False.</returns>
            public override bool Equals(object obj)
            {
                if (obj is Point)
                {
                    return Equals((Point)obj);
                }
                return false;
            }

            /// <summary>
            /// Indicates whether the current point equals to another point.
            /// </summary>
            /// <param name="v">Another point</param>
            /// <returns>True - if current point equals to another point, else - False.</returns>
            public bool Equals(Point point)
            {
                return this == point;
            }

            #endregion

            /// <summary>
            /// Is this point near point (0;0;0)?
            /// </summary>
            public bool IsOrigin { get { return ApproximatelyEqual(x, 0) && ApproximatelyEqual(y, 0) && ApproximatelyEqual(z, 0); } }
            
            #region INullable Members

            /// <summary>
            /// Is this a null point with coordinates (0;0;0)?
            /// </summary>
            public bool IsNull { get { return m_null; } }

            #endregion
            
            /// <summary>
            /// Build vector between two points (start in current point)
            /// </summary>
            /// <param name="end">Another point (vector end point)</param>
            /// <returns>New vector</returns>
            public Vector this[Point end]
            {
                get { return new Vector(end.x - x, end.y - y, end.z - z); }
            }

            /// <summary>
            /// Returns point into new Transformation
            /// </summary>
            /// <param name="matrix">Transform matrix</param>
            /// <returns></returns>
            public Point this[Transform transform]
            {
                get
                {
                    return transform.TransformPoint(this);
                }
            }
            /// <summary>
            /// Returns translated point at direction <paramref name="vactor"/>
            /// </summary>
            /// <param name="vector">translate direction</param>
            /// <returns></returns>
            public Point this[Vector vector] { get { return this + vector; } }

            #region operators

            /// <summary>
            /// Is point equal to object?
            /// </summary>
            /// <param name="p1">Point</param>
            /// <param name="p2">Object</param>
            /// <returns>True - if parameters are equal, else - False.</returns>
            public static bool operator ==(Point p1, object p2)
            {
                if (p2 == default(object))
                    return false;
                return p1 == (Point)p2;
            }

            /// <summary>
            /// Is point equal to object?
            /// </summary>
            /// <param name="p1">Point</param>
            /// <param name="p2">Object</param>
            /// <returns>True - if parameters are not equal, else - False.</returns>
            public static bool operator !=(Point p1, object p2)
            {
                if (p2 == default(object))
                    return true;
                return p1 != (Point)p2;
            }

            /// <summary>
            /// Compares two points
            /// </summary>
            /// <param name="p1">First point</param>
            /// <param name="p2">Second point</param>
            /// <returns>True - if difference between each coordinate less than accuracy, else - False.</returns>
            public static bool operator ==(Point p1, Point p2)
            {
                return
                   Math.Abs(p1.x - p2.x) < Accuracy
                  &&
                     Math.Abs(p1.y - p2.y) < Accuracy
                  &&
                    Math.Abs(p1.z - p2.z) < Accuracy;
            }

            /// <summary>
            /// Compares two points
            /// </summary>
            /// <param name="p1">First point</param>
            /// <param name="p2">Second point</param>
            /// <returns>True - if parameters are not equal, else - False.</returns>
            public static bool operator !=(Point p1, Point p2)
            {
                return !(p1 == p2);
            }

            /// <summary>
            /// Add point and vector coordinates
            /// </summary>
            /// <param name="point">Point</param>
            /// <param name="vector">Vector</param>
            /// <returns>New point</returns>
            public static Point operator +(Point point, Vector vector)
            {
                return new Point(point.x + vector.x, point.y + vector.y, point.z + vector.z);
            }

            /// <summary>
            /// Find difference between point and vector coordinates
            /// </summary>
            /// <param name="point">Point</param>
            /// <param name="vector">Vector</param>
            /// <returns>New point</returns>
            public static Point operator -(Point point, Vector vector)
            {
                return new Point(point.x - vector.x, point.y - vector.y, point.z - vector.z);
            }

            /// <summary>
            /// Set new vector between two points (difference)
            /// </summary>
            /// <param name="point1">First point</param>
            /// <param name="point2">Second point</param>
            /// <returns>New vector</returns>
            public static Vector operator -(Point point1, Point point2)
            {
                return new Vector(point1.x - point2.x, point1.y - point2.y, point1.z - point2.z);
            }

            /// <summary>
            /// Set new vector between two points (add)
            /// </summary>
            /// <param name="point1">First point</param>
            /// <param name="point2">Second point</param>
            /// <returns>New vector</returns>
            public static Vector operator +(Point point1, Point point2)
            {
                return new Vector(point1.x + point2.x, point1.y + point2.y, point1.z + point2.z);
            }

            #endregion

            #region static members

            /// <summary>
            /// Null Point
            /// </summary>
            public static Point Null
            {
                get
                {
                    Point p = new Point();
                    p.m_null = true;
                    return p;
                }
            }

            #endregion
        }
        #endregion

        #region Plane definition
        //[System.Diagnostics.DebuggerDisplay("{format(4)}", Name = "Plane")]
        [System.Runtime.InteropServices.StructLayout(System.Runtime.InteropServices.LayoutKind.Sequential)]
        public struct Plane :INullable
        {
            /// <summary>
            /// Matrix for plane equation
            /// </summary>
            private double[,] m_matr;

            /// <summary>
            /// Is this is a null plane?
            /// </summary>
            private bool m_null;

            /// <summary>
            /// Get plane information
            /// </summary>
            /// <param name="dig">Percision value</param>
            /// <returns>Plane information</returns>
            private string format(byte dig)
            {
                //Plane p = RoundTo(this, dig);
                //"(" + p.x.ToString() + "; " + p.y.ToString() + "; " + p.z.ToString() + ")";
                return string.Empty; 
            }            

            /// <summary>
            /// Initialize new plane
            /// </summary>
            /// <param name="normal">Vector value (normal)</param>
            /// <param name="point">Point value</param>
            public Plane(Vector normal, Point point)
                : this()
            {
                m_matr = new double[3, 3];

                Vector perp1 = normal.GetPerpVector();
                Vector perp2 = perp1 ^ normal;

                Point p1 = point, p2 = point + perp1, p3 = point + perp2;

                m_matr[0, 0] = p1.x;
                m_matr[0, 1] = p1.y;
                m_matr[0, 2] = p1.z;

                m_matr[1, 0] = p2.x - p1.x;
                m_matr[1, 1] = p2.y - p1.y;
                m_matr[1, 2] = p2.z - p1.z;

                m_matr[2, 0] = p3.x - p1.x;
                m_matr[2, 1] = p3.y - p1.y;
                m_matr[2, 2] = p3.z - p1.z;

            }

            /// <summary>
            /// Initialize new plane
            /// </summary>
            /// <param name="p1">First point</param>
            /// <param name="p2">Second point</param>
            /// <param name="p3">Third point</param>
            public Plane(Point p1, Point p2, Point p3)
                : this()
            {
                m_matr = new double[3, 3];

                m_matr[0, 0] = p1.x;
                m_matr[0, 1] = p1.y;
                m_matr[0, 2] = p1.z;

                m_matr[1, 0] = p2.x - p1.x;
                m_matr[1, 1] = p2.y - p1.y;
                m_matr[1, 2] = p2.z - p1.z;

                m_matr[2, 0] = p3.x - p1.x;
                m_matr[2, 1] = p3.y - p1.y;
                m_matr[2, 2] = p3.z - p1.z;
            }

            /// <summary>
            /// Initialize new plane
            /// </summary>
            /// <param name="v1">Non-collinear vector value</param>
            /// <param name="v2">Non-collinear vector value</param>
            /// <param name="p">Point value</param>
            public Plane(Vector v1, Vector v2, Point p): this()
            {
                m_matr = new double[3, 3];
                Point p1 = p, p2 = p + v1, p3 = p + v2;

                m_matr[0, 0] = p1.x;
                m_matr[0, 1] = p1.y;
                m_matr[0, 2] = p1.z;

                m_matr[1, 0] = p2.x - p1.x;
                m_matr[1, 1] = p2.y - p1.y;
                m_matr[1, 2] = p2.z - p1.z;

                m_matr[2, 0] = p3.x - p1.x;
                m_matr[2, 1] = p3.y - p1.y;
                m_matr[2, 2] = p3.z - p1.z;
            }

            /// <summary>
            /// Is plane contain vector?
            /// </summary>
            /// <param name="vector">Vector value</param>
            /// <returns>True - if plane contains vector, else - False.</returns>
            public bool Contains(Vector vector)
            {
                return Contains(new Point(m_matr[0, 0], m_matr[0, 1], m_matr[0, 2])[vector]);
            }

            /// <summary>
            /// Is plane contain point?
            /// </summary>
            /// <param name="point">Point value</param>
            /// <returns>True - if plane contains point, else - False.</returns>
            public bool Contains(Point point)
            {
                double a = point.x - m_matr[0, 0];
                double b = point.y - m_matr[0, 1];
                double c = point.z - m_matr[0, 2];

                double det = a * m_matr[1, 1] * m_matr[2, 2];
                det += b * m_matr[1, 2] * m_matr[2, 0];
                det += c * m_matr[1, 0] * m_matr[2, 1];
                det -= c * m_matr[1, 1] * m_matr[2, 0];
                det -= b * m_matr[1, 0] * m_matr[2, 2];
                det -= a * m_matr[1, 2] * m_matr[2, 1];

                return ApproximatelyEqual(det, 0.0);
            }

            /// <summary>
            /// Calculate distance from plane to point
            /// </summary>
            /// <param name="point">Point</param>
            /// <returns>Distance value</returns>
            public double DistanceTo(Point point)
            {
                double[] x = new double[3];
                double[] y = new double[3];
                double[] z = new double[3];

                for (int i = 0; i < 3; ++i)
                {
                    x[i] = m_matr[i, 0];
                    y[i] = m_matr[i, 1];
                    z[i] = m_matr[i, 2];
                }

                double A = y[1] * z[2] - z[1] * y[2];
                double B = x[2] * z[1] - x[1] * z[2];
                double C = x[1] * y[2] - x[2] * y[1];

                double D = x[0] * (z[1] * y[2] - y[1] * z[2]);
                D += x[1] * (z[2] * y[0] - y[2] * z[0]);
                D += x[2] * (y[1] * z[0] - z[1] * y[0]);

                double retval = (A * point.x) + (B * point.y) + (C * point.z) + D;
                retval /= Math.Sqrt((A * A) + (B * B) + (C * C));

                return Math.Abs(retval);
            }

            /// <summary>
            /// Calculate distance between two planes
            /// </summary>
            /// <param name="plane">Another plane</param>
            /// <returns>Distance value</returns>
            public double DistanceTo(Plane plane)
            {
                return DistanceTo(new Point(plane.m_matr[0, 0], plane.m_matr[0, 1], m_matr[0, 2]));
            }

            /// <summary>
            /// Points array from plane
            /// </summary>
            public Point[] points
            {
                get
                {
                    Point[] retval = new Point[3];
                    retval[0] = new Point(m_matr[0, 0], m_matr[0, 1], m_matr[0, 2]);

                    Vector vec = new Vector(m_matr[1, 0], m_matr[1, 1], m_matr[1, 2]);
                    retval[1] = retval[0] + vec;

                    vec = new Vector(m_matr[2, 0], m_matr[2, 1], m_matr[2, 2]);
                    retval[2] = retval[0] + vec;

                    return retval;
                }
            }

            /// <summary>
            /// Find intersection between this plane and vector
            /// </summary>
            /// <param name="a">Vector start point</param>
            /// <param name="b">Vector end point</param>
            /// <returns>Intersection point value.</returns>
            private Point intersect(Point a, Point b)
            {
                //sometimes (like in th bug #2307) we have very large inaccuracy
                //so we need to square our angle to prevent the error 
                double angle = this.AngleTo(a[b]);
                if (ApproximatelyEqual(Math.Pow(angle, 2), 0.0))
                    return Point.Null;

                Point p = new Point(m_matr[0, 0], m_matr[0, 1], m_matr[0, 2]);
                Vector l = new Vector(m_matr[1, 0], m_matr[1, 1], m_matr[1, 2]);
                Vector m = new Vector(m_matr[2, 0], m_matr[2, 1], m_matr[2, 2]);
                Vector val = l ^ m;
                Point q = a + ((b - a) * (((p - a) * val) / ((b - a) * val)));
                return q;
            }

            /// <summary>
            /// Get intersection points array
            /// </summary>
            /// <param name="plane">Another plane</param>
            /// <returns>Intersection points array.</returns>
            public Point[] Intersect(Plane plane)
            {
                Point[] ret = new Point[2];
                Point[] pts = points;
                List<Point[]> lines1 = new List<Point[]>();
                lines1.Add(new Point[] { pts[0], pts[1]});
                lines1.Add(new Point[] { pts[1], pts[2] });
                lines1.Add(new Point[] { pts[2], pts[0] });

                int k = 0;
                for (int i = 0; i < lines1.Count; ++i)
                    if (!plane.IsParallel(lines1[i][1][lines1[i][0]]) && k != 2)
                    {
                        ret[k] = plane.intersect(lines1[i][0], lines1[i][1]);
                        k++;
                    }

                if (k != 2)
                    return new Point[0];

                return ret;
            }

            public Vector ParallelVector() { return (Normal ^ new Vector(0, 0, 1)).GetNormalized(); }

            /// <summary>
            /// Get normal to plane
            /// </summary>
            public Vector Normal
            {
                get
                {
                    double[] x = new double[3];
                    double[] y = new double[3];
                    double[] z = new double[3];

                    for (int i = 0; i < 3; ++i)
                    {
                        x[i] = m_matr[i, 0];
                        y[i] = m_matr[i, 1];
                        z[i] = m_matr[i, 2];
                    }

                    double A = y[1] * z[2] - z[1] * y[2];
                    double B = x[2] * z[1] - x[1] * z[2];
                    double C = x[1] * y[2] - x[2] * y[1];

                    return (new Vector(A, B, C).GetNormalized());
                }
            }
            
            /// <summary>
            /// Are two plane parallel?
            /// </summary>
            /// <param name="p">Another plane</param>
            /// <returns>True - if this plane is parallel to another one, else - False.</returns>
            public bool IsParallel(Plane p)
            {
                return Normal.IsParallelTo(p.Normal);
            }

            /// <summary>
            /// Compare normal and aother vector on parallel.
            /// </summary>
            /// <param name="vector">Another vector</param>
            /// <returns>True - if normal and  vector are parallel, else - False.</returns>
            public bool IsParallel(Vector vector)
            {
                double angle = 0.0;
                angle = Normal.AngleTo(vector);
                return ApproximatelyEqual(angle, Math.PI) || ApproximatelyEqual(angle, 0) ;
            }

            /// <summary>
            /// Find angle between normal and another vector
            /// </summary>
            /// <param name="vector">Another vector</param>
            /// <returns>Andle in radians.</returns>
            public double AngleTo(Vector vector)
            {
                double angle = Normal.AngleTo(vector);
                angle = (angle < (Math.PI / 2)) ? (Math.PI / 2 - angle) : angle - (Math.PI / 2);
                return angle;
            }
            
            #region INullable Members

            public bool IsNull { get { return m_null; } }

            #endregion
            
            #region Projection

            /// <summary>
            /// Find point projection on plane.
            /// </summary>
            /// <param name="point">Point value</param>
            /// <returns>Projection point value.</returns>
            public Point Projection(Point point)
            {
                double dist = DistanceTo(point);
                Vector norm = Normal.GetNormalized();
                Point pr = point - (norm * dist);
                if (!ApproximatelyEqual(DistanceTo(pr), 0))
                    pr = point + (norm * dist);
                return pr;
            }

            /// <summary>
            /// Find vector projection on plane.
            /// </summary>
            /// <param name="vector">Vector value</param>
            /// <returns>Projection vector value.</returns>
            public Vector Projection(Vector vector)
            {
                Point p1 = new Point(m_matr[0, 0], m_matr[0, 1], m_matr[0, 2]);
                Point p2 = Projection(p1[vector]);
                return p1[p2];
            }
            #endregion

            #region Shift

            /// <summary>
            /// Move plane
            /// </summary>
            /// <param name="direction">Direction vector</param>
            public void Shift(Vector direction)
            {
                Point
                    p1 = new Point(m_matr[0, 0], m_matr[0, 1], m_matr[0, 2]),
                    p2 = new Point(m_matr[1, 0] + p1.x, m_matr[1, 1] + p1.y, m_matr[1, 2] + p1.z),
                    p3 = new Point(m_matr[2, 0] + p1.x, m_matr[2, 1] + p1.y, m_matr[2, 2] + p1.z);

                p1 = p1 + direction;
                p2 = p2 + direction;
                p3 = p3 + direction;

                m_matr[0, 0] = p1.x;
                m_matr[0, 1] = p1.y;
                m_matr[0, 2] = p1.z;

                m_matr[1, 0] = p2.x - p1.x;
                m_matr[1, 1] = p2.y - p1.y;
                m_matr[1, 2] = p2.z - p1.z;

                m_matr[2, 0] = p3.x - p1.x;
                m_matr[2, 1] = p3.y - p1.y;
                m_matr[2, 2] = p3.z - p1.z;
            }

            /// <summary>
            /// Move this plane on distance
            /// </summary>
            /// <param name="distance">Distance value</param>
            public void Shift(double distance)
            {
                Shift(Normal * distance);
            }

            /// <summary>
            /// Get moved plane
            /// </summary>
            /// <param name="direction">Direction value</param>
            /// <returns>Moved plane.</returns>
            public Plane GetShifted(Vector direction)
            {
                Plane plane = this;
                plane.Shift(direction);
                return plane;
            }

            public Plane GetShifted(double distance)
            {
                Plane plane = this;
                plane.Shift(distance);
                return plane;
            }
            #endregion

            /// <summary>
            /// Inspect vector position
            /// </summary>
            /// <param name="v">Vector value</param>
            /// <returns>Vector position on coordinate plane.</returns>
            public DecartPosition DetectDecartPart(Vector v)
            {
                if (
                    ApproximatelyEqual(v.x, 0)
                  & ApproximatelyEqual(v.y, 0)
                  & ApproximatelyEqual(v.z, 0))
                    return DecartPosition.Origin;

                if (v.x > 0 & v.y > 0)
                    return DecartPosition.At1;
                if (v.x > 0 & v.y < 0)
                    return DecartPosition.At2;
                if (v.x < 0 & v.y < 0)
                    return DecartPosition.At3;
                if (v.x < 0 & v.y > 0)
                    return DecartPosition.At4;

                if (v.x > 0 & v.y == 0)
                    return DecartPosition.AtY_PlusX;
                if (v.x < 0 & v.y == 0)
                    return DecartPosition.AtY_MinusX;

                if (v.x == 0 & v.y > 0)
                    return DecartPosition.AtX_PlusY;
                if (v.x == 0 & v.y < 0)
                    return DecartPosition.AtX_MinusY;
                
                throw new InvalidOperationException();
            }
            public DecartPosition DetectDecartPart(Point p)
            {
                if (
                    ApproximatelyEqual(p.x, 0)
                  & ApproximatelyEqual(p.y, 0)
                  & ApproximatelyEqual(p.z, 0))
                    return DecartPosition.Origin;

                if (p.x > 0 & p.y > 0)
                    return DecartPosition.At1;
                if (p.x > 0 & p.y < 0)
                    return DecartPosition.At2;
                if (p.x < 0 & p.y < 0)
                    return DecartPosition.At3;
                if (p.x < 0 & p.y > 0)
                    return DecartPosition.At4;

                if (p.x > 0 & p.y == 0)
                    return DecartPosition.AtY_PlusX;
                if (p.x < 0 & p.y == 0)
                    return DecartPosition.AtY_MinusX;

                if (p.x == 0 & p.y > 0)
                    return DecartPosition.AtX_PlusY;
                if (p.x == 0 & p.y < 0)
                    return DecartPosition.AtX_MinusY;

                throw new InvalidOperationException();
            }

            #region static members
            public static Plane Null
            {
                get
                {
                    Plane p = new Plane();
                    p.m_null = true;
                    return p;
                }
            }
            #endregion

        }
        #endregion

        #region Quaterion definition
        //[System.Diagnostics.DebuggerDisplay("{format(8)}", Name = "Quaterion")]
        [System.Runtime.InteropServices.StructLayout(System.Runtime.InteropServices.LayoutKind.Sequential)]
        public struct Quaterion : INullable
        {
            private Vector vector;
            private double w;
            private bool m_null;


            private string format(byte dig)
            {
                Quaterion q = RoundTo(this, dig);
                return "(" + q.vector.x.ToString() + "; " + q.vector.y.ToString() + "; " + q.vector.z.ToString() + ";" + q.w.ToString() + "), Length:" + q.vector.Length().ToString();
            }

            public Quaterion(double x, double y, double z, double w)
                : this()
            {
                vector = new Vector(x, y, z);
                this.w = w;
            }

            public Quaterion(double x, double y, double z) : this(x, y, z, 0) { }
            /// <summary>
            /// 
            /// </summary>
            /// <param name="axis">Rotation axis</param>
            /// <param name="angle">In radians</param>
            public Quaterion(Vector axis, double angle) : this(axis.x, axis.y, axis.z, angle) { }
            /// <summary>
            /// Length of the quaterion
            /// </summary>
            public double Norma
            {
                get { return vector.Length2() + w*w; }
            }

            public Vector Vector
            {
                get { return vector; }
                set { vector = value; }
            }

            public double W
            {
                get { return w; }
                set { w = value; }
            }
            public double Magnitude { get { return Math.Sqrt(Norma); } }

            public Quaterion Conjugate
            {
                get
                {
                    Quaterion q = this;
                    q.vector = vector.GetInverse();
                    return q;
                }
            }

            public Quaterion Negate
            {
                get { return new Quaterion(-vector.x, -vector.y, -vector.z, -w); }
            }

            public double Angle { get { return Math.Acos(w) * 2.0; } }

            public void FromAngleAxis(double Angle, Vector axis)	// set the Quaternion by Angle-axis
            {
                double x = axis.x;
                double y = axis.y;
                double z = axis.z;

                // required: Normalize the axis

                double i_length = 1.0 / Math.Sqrt(x * x + y * y + z * z);

                x = x * i_length;
                y = y * i_length;
                z = z * i_length;

                // now make a clQuaternionernion out of it
                double Half = DegToRad(Angle * 0.5);

                w = Math.Cos(Half);//this used to be w/o deg to rad.

                double sin_theta_over_two = Math.Sin(Half);
                x = x * sin_theta_over_two;
                y = y * sin_theta_over_two;
                z = z * sin_theta_over_two;

                vector = new Vector(x, y, z);
            }

            public void FromAngleAxis2(double AngleRadians, Vector axis)
            {
                double s;
                s = Math.Sin(AngleRadians * 0.5f);
                w = Math.Cos(AngleRadians * 0.5f);
                vector.x = axis.x * s;
                vector.y = axis.y * s;
                vector.z = axis.z * s;
            }

            public void Normalize()
            {
                double mag = Norma;
                if (mag> 0)
                {
                    double factor = 1.0 / Math.Sqrt(mag);

                    vector.x *= factor;
                    vector.y *= factor;
                    vector.z *= factor;
                    w *= factor;
                }
            }

            public void Slerp(double t, Quaterion left, Quaterion q2)
            {
                double quatEpsilon = 1.0e-8;

                vector = left.vector;
                w = left.w;

                double cosine =
                    vector.x * q2.vector.x +
                    vector.y * q2.vector.y +
                    vector.z * q2.vector.z +
                    w * q2.w;		//this is left.dot(right)

                double sign = 1.0;
                if (cosine < 0)
                {
                    cosine = -cosine;
                    sign = -1.0;
                }

                double Sin = 1.0 - cosine * cosine;

                if (Sin >= quatEpsilon * quatEpsilon)
                {
                    Sin = Math.Sqrt(Sin);
                    double _angle = Math.Atan2(Sin, cosine);
                    double i_sin_angle = 1.0 / Sin;



                    double lower_weight = Math.Sin(_angle * (1.0 - t)) * i_sin_angle;
                    double upper_weight = Math.Sin(_angle * t) * i_sin_angle * sign;

                    w = (w * (lower_weight)) + (q2.w * (upper_weight));
                    vector.x = (vector.x * (lower_weight)) + (q2.vector.x * (upper_weight));
                    vector.y = (vector.y * (lower_weight)) + (q2.vector.y * (upper_weight));
                    vector.z = (vector.z * (lower_weight)) + (q2.vector.z * (upper_weight));
                }
            }
            /// <summary>
            /// returns axis and angle of rotation of quaternion
            /// </summary>
            /// <param name="angle"></param>
            /// <param name="axis"></param>
            public void GetAngleAxis(out double angle, ref Vector axis)
            {                
                angle = Math.Acos(w) * 2.0;
                angle = RadToDeg(angle);
                double sa = Math.Sqrt(1.0 - w * w);
                if (sa != 0)
                {
                    sa = 1 / sa;
                    axis.x = vector.x * sa;
                    axis.y = vector.y * sa;
                    axis.z = vector.z * sa;
                }
                else
                {
                    axis.x = 1.0;
                    axis.y = 0.0;
                    axis.z = 0.0;
                }
            }

            public void Round(uint digits)
            {
                double factor1 = default(double);
                double factor2 = factor1;
                if (Factor(digits, out factor1, out factor2))
                {
                    vector.x = (int)vector.x;
                    vector.y = (int)vector.y;
                    vector.z = (int)vector.z;
                    w = (int)w;
                }
                else
                {
                    vector.x = ((int)(vector.x * factor2)) * factor1;
                    vector.y = ((int)(vector.y * factor2)) * factor1;
                    vector.z = ((int)(vector.z * factor2)) * factor1;
                    w = ((int)(w* factor2)) * factor1;
                }
            }       

            public Vector Rotate(Vector v)
            {
                Vector qv = vector;
                return (v * (w * w - 0.5) + (qv ^ v) * w + qv * (qv * v)) * 2;
            }
            public Vector InvRotate(Vector v)
            {
                Vector qv = vector;
                return (v * (w * w - 0.5) - (qv ^ v) * w + qv * (qv * v)) * 2;
            }

            public Vector Transform(Vector vec, Vector position) 
            {
                return Rotate(vec) + position;
            }
            public Vector InvTransform(Vector vec, Vector position)
            {
                return InvRotate(vec - position);
            }


            //public void Multiply(

            #region static members

            public static Quaterion operator +(Quaterion q1, Quaterion q2) 
            {
                return new Quaterion(q1.vector.x + q2.vector.x, q1.vector.y + q2.vector.y, q1.vector.z + q2.vector.z, q1.w + q2.w);
            }
            
            public static Quaterion operator -(Quaterion q1, Quaterion q2)
            {
                return new Quaterion(q1.vector.x - q2.vector.x, q1.vector.y - q2.vector.y, q1.vector.z - q2.vector.z, q1.w - q2.w);
            }

            public static Quaterion operator ^(Quaterion q1, Quaterion q2)
            {
                double a, b, c, d;

                a = q1.w * q2.w - q1.vector.x * q2.vector.x - q1.vector.y * q2.vector.y - q1.vector.z * q2.vector.z;
                b = q1.w * q2.vector.x + q2.w * q1.vector.x + q1.vector.y * q2.vector.z - q2.vector.y * q1.vector.z;
                c = q1.w * q2.vector.y + q2.w * q1.vector.y + q1.vector.z * q2.vector.x - q2.vector.z * q1.vector.x;
                d = q1.w * q2.vector.z + q2.w * q1.vector.z + q1.vector.x * q2.vector.y - q2.vector.x * q1.vector.y;
                return new Quaterion(b, c, d, a);
            }

            public static Quaterion operator ^(Quaterion q1, Vector q2)
            {
                double a, b, c, d;

                a = -q1.vector.x * q2.x - q1.vector.y * q2.y - q1.vector.z * q2.z;
                b = q1.w * q2.x + q1.vector.y * q2.z - q2.y * q1.vector.z;
                c = q1.w * q2.y + q1.vector.z * q2.x - q2.z * q1.vector.x;
                d = q1.w * q2.z + q1.vector.x * q2.y - q2.x * q1.vector.y;
                return new Quaterion(b, c, d, a);
            }

            public static double operator *(Quaterion q1, Quaterion q2)
            {
                return q1.vector.x * q2.vector.x + q1.vector.y * q2.vector.y + q1.vector.z * q2.vector.z + q1.w * q2.w;
            }
           
            public static Quaterion Null
            {
                get
                {
                    Quaterion q = new Quaterion();
                    q.m_null = true;
                    return q;
                }
            }

            public static Quaterion Identity { get { return new Quaterion(0, 0, 0, 1); } }

            #endregion

            #region INullable Members

            public bool IsNull
            {
                get { return m_null || vector.IsNull; }
            }

            #endregion
        }
        #endregion

        #region Matrix definition
       // [System.Diagnostics.DebuggerDisplay("{format(8)}", Name = "Matrix3")]
        [System.Runtime.InteropServices.StructLayout(System.Runtime.InteropServices.LayoutKind.Sequential)]
        public struct Matrix3: INullable, IEquatable<Matrix3>
        {
            private double[,] m_matrix;
            private bool m_null;

            private string format(byte dig)
            {
                return string.Empty;
            }

            /// <summary>
            /// 
            /// </summary>
            /// <param name="data"></param>
            public Matrix3(double[,] data)
            {
                m_null = false;
                m_matrix = new double[3, 3]
               {
                   {data[0,0], data[0,1], data[0,2]},
                   {data[1,0], data[1,1], data[1,2]},
                   {data[2,0], data[2,1], data[2,2]}
               };
            }

            /// <summary>
            /// 
            /// </summary>
            /// <param name="data"></param>
            public Matrix3(double[] data)
            {
                m_null = false;
                m_matrix = new double[3, 3]
               {
                   {data[0], data[1], data[2]},
                   {data[3], data[4], data[5]},
                   {data[6], data[7], data[8]}
               };
            }

            /// <summary>
            /// 
            /// </summary>
            /// <param name="matrix"></param>
            public Matrix3(Matrix3 matrix) : this(matrix.m_matrix) { }

            /// <summary>
            /// 
            /// </summary>
            /// <param name="q"></param>
            public Matrix3(Quaterion q):this()
            {
                m_matrix = new double[3,3];
                double w = q.W;
                double x = q.Vector.x;
                double y = q.Vector.y;
                double z = q.Vector.z;

                m_matrix[0,0] = 1.0 - y * y * 2.0 - z * z * 2.0;
                m_matrix[0, 1] = x * y * 2.0 - w * z * 2.0;
                m_matrix[0, 2] = x * z * 2.0 + w * y * 2.0;

                m_matrix[1, 0] = x * y * 2.0 + w * z * 2.0;
                m_matrix[1, 1] = 1.0 - x * x * 2.0 - z * z * 2.0;
                m_matrix[1, 2] = y * z * 2.0 - w * x * 2.0;

                m_matrix[2, 0] = x * z * 2.0 - w * y * 2.0;
                m_matrix[2, 1] = y * z * 2.0 + w * x * 2.0;
                m_matrix[2, 2] = 1.0 - x * x * 2.0 - y * y * 2.0;
            }
            /// <summary>
            /// Creates matrix from Euler angles
            /// </summary>
            /// <param name="X"></param>
            /// <param name="Y"></param>
            /// <param name="Z"></param>
            public Matrix3(double X, double Y, double Z): this()
            {
                m_matrix = new double[3, 3];

                X = DegToRad(X);
                Y = DegToRad(Y);
                Z = DegToRad(Z);

                Matrix3 RotZ = new Matrix3(
                    new Vector(Math.Cos(Z), -Math.Sin(Z), 0),
                    new Vector(Math.Sin(Z), Math.Cos(Z), 0),
                    new Vector(0, 0, 1));

                Matrix3 RotY = new Matrix3(
                    new Vector(Math.Cos(Y), 0, Math.Sin(Y)),
                    new Vector(0, 1, 0),
                    new Vector(-Math.Sin(Y), 0, Math.Cos(Y)));

                Matrix3 RotX = new Matrix3(
                    new Vector(1, 0, 0),
                    new Vector(0, Math.Cos(X), -Math.Sin(X)),
                    new Vector(0, Math.Sin(X), Math.Cos(X)));

                m_matrix = (RotX * RotY * RotZ).m_matrix;
            }
            /// <summary>
            /// Creates matrix from rotation an axis
            /// </summary>
            /// <param name="XX">Rotation at X-Axis</param>
            /// <param name="YY">Rotation at Y-Axis</param>
            /// <param name="ZZ">Rotation at Z-Axis</param>
            public Matrix3(Vector XX, Vector YY, Vector ZZ): this()
            {
                m_matrix = new double[3, 3];
                m_matrix[0, 0] = XX.x;
                m_matrix[0, 1] = XX.y;
                m_matrix[0, 2] = XX.z;

                m_matrix[1, 0] = YY.x;
                m_matrix[1, 1] = YY.y;
                m_matrix[1, 2] = YY.z;

                m_matrix[2, 0] = ZZ.x;
                m_matrix[2, 1] = ZZ.y;
                m_matrix[2, 2] = ZZ.z;
            }

            /// <summary>
            /// Get column from matrix
            /// </summary>
            /// <param name="column">Column number</param>
            /// <returns>Vector value</returns>
            public Vector Column_get(int column)
            {
                return new Vector(
                    m_matrix[0, column]
                   ,m_matrix[1, column]
                   ,m_matrix[2, column]
                   );
            }

            /// <summary>
            /// Set column by vector value
            /// </summary>
            /// <param name="column">Column number</param>
            /// <param name="value">Vector value</param>
            public void Column_set(int column, Vector value)
            {
                m_matrix[0, column] = value.x;
                m_matrix[1, column] = value.y;
                m_matrix[2, column] = value.z;
            }

            /// <summary>
            /// Get row from matrix
            /// </summary>
            /// <param name="row">Row number</param>
            /// <returns>Vector value</returns>
            public Vector Row_get(int row)
            {
                return new Vector(
                    m_matrix[row, 0]
                   ,m_matrix[row, 1]
                   ,m_matrix[row, 2]);
            }

            /// <summary>
            /// Set row by vector value
            /// </summary>
            /// <param name="row">Row number</param>
            /// <param name="value">Vector value</param>
            public void Row_set(int row, Vector value)
            {

                m_matrix[row, 0] = value.x;
                m_matrix[row, 1] = value.y;
                m_matrix[row, 2] = value.z;
            }

            /// <summary>
            /// Get matrix in array by rows
            /// </summary>
            /// <returns>Array value</returns>
            public double[] RowMajor_get()
            {
                double[] ret = new double[9];

                ret[0] = m_matrix[0, 0];
                ret[1] = m_matrix[0, 1];
                ret[2] = m_matrix[0, 2];

                ret[3] = m_matrix[1, 0];
                ret[4] = m_matrix[1, 1];
                ret[5] = m_matrix[1, 2];

                ret[6] = m_matrix[2, 0];
                ret[7] = m_matrix[2, 1];
                ret[8] = m_matrix[2, 2];

                return ret;
            }

            /// <summary>
            /// Set matrix rows by array
            /// </summary>
            /// <param name="ret">Array value</param>
            public void RowMajor_set(double[] ret)
            {
                m_matrix[0, 0] = ret[0];
                m_matrix[0, 1] = ret[1];
                m_matrix[0, 2] = ret[2];

                m_matrix[1, 0] = ret[3];
                m_matrix[1, 1] = ret[4];
                m_matrix[1, 2] = ret[5];

                m_matrix[2, 0] = ret[6];
                m_matrix[2, 1] = ret[7];
                m_matrix[2, 2] = ret[8];
            }

            /// <summary>
            /// 
            /// </summary>
            /// <returns></returns>
            public double[] ColumnMajor_get() 
            {
                double[] ret = new double[9];

                ret[0] = m_matrix[0, 0];
                ret[1] = m_matrix[1, 0];
                ret[2] = m_matrix[2, 0];

                ret[3] = m_matrix[0, 1];
                ret[4] = m_matrix[1, 1];
                ret[5] = m_matrix[2, 1];

                ret[6] = m_matrix[0, 2];
                ret[7] = m_matrix[1, 2];
                ret[8] = m_matrix[2, 2];

                return ret;
            }
            /// <summary>
            /// 
            /// </summary>
            /// <param name="ret"></param>
            public void ColumnMajor_set(double[] ret)
            {
                m_matrix[0, 0] = ret[0];
                m_matrix[1, 0] = ret[1];
                m_matrix[2, 0] = ret[2];

                m_matrix[0, 1] = ret[3];
                m_matrix[1, 1] = ret[4];
                m_matrix[2, 1] = ret[5];

                m_matrix[0, 2] = ret[6];
                m_matrix[1, 2] = ret[7];
                m_matrix[2, 2] = ret[8];
            }

            /// <summary>
            /// Get main diagonal
            /// </summary>
            public double[] Diag_Main
            {
                get
                {
                    return new double[] { m_matrix[0, 0], m_matrix[1, 1], m_matrix[2, 2] };
                }
            }
            
            public double this[int row, int column]
            {
                get { return m_matrix[row, column]; }
                set { m_matrix[row, column] = value; }
            }

            /// <summary>
            /// Data of the matrix
            /// </summary>
            public double[] Data{
                get
                {
                    return new double[] {
                        m_matrix[0, 0], m_matrix[0, 1], m_matrix[0, 2]
                       ,m_matrix[1, 0], m_matrix[1, 1], m_matrix[1, 2]
                       ,m_matrix[2, 0], m_matrix[2, 1], m_matrix[2, 2]
                    };
                }
                set{
                   m_matrix[0, 0] = value[0]; m_matrix[0, 1] = value[1]; m_matrix[0, 2] = value[2];
                   m_matrix[1, 0] = value[3]; m_matrix[1, 1] = value[4]; m_matrix[1, 2] = value[5];
                   m_matrix[2, 0] = value[6]; m_matrix[2, 1] = value[7]; m_matrix[2, 2] = value[8];
                }
            }
            /// <summary>
            /// Returns the Determinant of this matrix
            /// </summary>
            public double Determinant
            {
                get
                {
                    return m_matrix[0, 0] * m_matrix[1, 1] * m_matrix[2, 2] + m_matrix[0, 1] * m_matrix[1, 2] * m_matrix[2, 0] + m_matrix[0, 2] * m_matrix[1, 0] * m_matrix[2, 1]
                      - m_matrix[0, 2] * m_matrix[1, 1] * m_matrix[2, 0] - m_matrix[0, 1] * m_matrix[1, 0] * m_matrix[2, 2] - m_matrix[0, 0] * m_matrix[1, 2] * m_matrix[2, 1];
                }
            }
            /// <summary>
            /// Transorm this matrix to quaterion
            /// </summary>
            public Quaterion Quaterion
            {
                get
                {
                    Quaterion q = new Quaterion();
                    double tr, s;
                    double x, y, z;
                    tr = m_matrix[0, 0] + m_matrix[1, 1] + m_matrix[2, 2];
                    if (tr >= 0)
                    {
                        s = Math.Sqrt(tr + 1);
                        q.W = 0.5 * s;
                        s = 0.5 / s;
                        x = (this[2, 1] - this[1, 2]) * s;
                        y = (this[0, 2] - this[2, 0]) * s;
                        z = (this[1, 0] - this[0, 1]) * s;
                        q.Vector = new Vector(x, y, z);
                    }
                    else
                    {
                        int i = 0;
                        if (m_matrix[1, 1] > m_matrix[0, 0])
                            i = 1;
                        if (m_matrix[2, 2] > m_matrix[i, i])
                            i = 2;
                        switch (i)
                        {
                            case 0:
                                {
                                    s = Math.Sqrt((m_matrix[0, 0] - (m_matrix[1, 1] + m_matrix[2, 2])) + 1);
                                    x = 0.5 * s;
                                    s = 0.5 / s;
                                    y = (m_matrix[0, 1] + m_matrix[1, 0]) * s;
                                    z = (m_matrix[2, 0] + m_matrix[0, 2]) * s;
                                    q.W = (m_matrix[2, 1] - m_matrix[1, 2]) * s;
                                    q.Vector = new Vector(x, y, z);
                                    break;
                                }
                            case 1:
                                {
                                    s = Math.Sqrt((m_matrix[1, 1] - (m_matrix[2, 2] + m_matrix[0, 0])) + 1);
                                    y = 0.5 * s;
                                    s = 0.5 / s;
                                    z = (this[1, 2] + this[2, 1]) * s;
                                    x = (this[0, 1] + this[1, 0]) * s;
                                    q.W = (this[0, 2] - this[2, 0]) * s;
                                    break;
                                }
                            case 2:
                                {
                                    s = Math.Sqrt((m_matrix[2, 2] - (m_matrix[0, 0] + m_matrix[1, 1])) + 1);
                                    z = 0.5 * s;
                                    s = 0.5 / s;
                                    x = (this[2, 0] + this[0, 2]) * s;
                                    y = (this[1, 2] + this[2, 1]) * s;
                                    q.W = (this[1, 0] - this[0, 1]) * s;
                                    break;
                                }
                        }
                    }
                    return q;
                }
            }
            /// <summary>
            /// Get opposite matrix of this matrix
            /// </summary>
            public Matrix3 Inverse
            {
                get
                {
                    double d = Determinant;

                    if (ApproximatelyEqual(d, 0.0))
                    {                        
                        return Null;
                    }

                    d = 1.0 / d;
                    
                    double AlgExt11, AlgExt12, AlgExt13, AlgExt21, AlgExt22, AlgExt23, AlgExt31, AlgExt32, AlgExt33;
                    AlgExt11 = m_matrix[1, 1] * m_matrix[2, 2] - m_matrix[1, 2] * m_matrix[2, 1];
                    AlgExt21 = m_matrix[0, 2] * m_matrix[2, 1] - m_matrix[0, 1] * m_matrix[2, 2];
                    AlgExt31 = m_matrix[0, 1] * m_matrix[1, 2] - m_matrix[0, 2] * m_matrix[1, 1];
                    AlgExt12 = m_matrix[1, 2] * m_matrix[2, 0] - m_matrix[1, 0] * m_matrix[2, 2];
                    AlgExt22 = m_matrix[0, 0] * m_matrix[2, 2] - m_matrix[0, 2] * m_matrix[2, 0];
                    AlgExt32 = m_matrix[0, 2] * m_matrix[1, 0] - m_matrix[0, 0] * m_matrix[1, 2];
                    AlgExt13 = m_matrix[1, 0] * m_matrix[2, 1] - m_matrix[1, 1] * m_matrix[2, 0];
                    AlgExt23 = m_matrix[0, 1] * m_matrix[2, 0] - m_matrix[0, 0] * m_matrix[2, 1];
                    AlgExt33 = m_matrix[0, 0] * m_matrix[1, 1] - m_matrix[0, 1] * m_matrix[1, 0];

                    Matrix3 dest = new Matrix3();
                    dest.m_matrix = new double[,]{
                        {AlgExt11 * d, AlgExt21 * d, AlgExt31 * d},
                        {AlgExt12 * d, AlgExt22 * d, AlgExt32 * d},
                        {AlgExt13 * d, AlgExt23 * d, AlgExt33 * d},
                    };

                    ////Matrix3 dest = new Matrix3(new double[] { 
                    ////    AlgExt11 * d, AlgExt21 * d, AlgExt31 * d,
                    ////    AlgExt12 * d, AlgExt22 * d, AlgExt32 * d,
                    ////    AlgExt13 * d, AlgExt23 * d, AlgExt33 * d
                    ////});
                    
                    return dest;
                }
            }
            /// <summary>
            /// true if this matrix is identity
            /// </summary>
            public bool IsIdentity
            {
                get
                {
                    for (int i = 0; i < 3; i++)
                        for (int j = 0; j < 3; j++)
                            if (i != j)
                            { if (!ApproximatelyEqual(m_matrix[i, j], 0)) return false; }
                            else
                            { if (!ApproximatelyEqual(m_matrix[i, j], 1)) return false; }
                    
                    return true;
                }
            }
            /// <summary>
            /// True if this matrix is nullable
            /// </summary>
            public bool IsZero
            {
                get
                {
                    for (int i = 0; i < 3; i++)
                        for (int j = 0; j < 3; j++)
                            if (!ApproximatelyEqual(m_matrix[i, j], 0)) return false;
                    return true;
                }
            }

            public Vector XAxis { get { return new Vector(m_matrix[0, 0], m_matrix[0, 1], m_matrix[0, 2]); } }
            public Vector YAxis { get { return new Vector(m_matrix[1, 0], m_matrix[1, 1], m_matrix[1, 2]); } }
            public Vector ZAxis { get { return new Vector(m_matrix[2, 0], m_matrix[2, 1], m_matrix[2, 2]); } }

            /// <summary>
            /// Makes this matrix as nullable
            /// </summary>
            public void Zero()
            {
                for (int i = 0; i < 3; i++)
                    for (int j = 0; j < 3; j++)
                        m_matrix[i, j] = 0;
            }
            /// <summary>
            /// Negate this matrix
            /// </summary>
            public void Negate()
            {
                for (int i = 0; i < 3; i++)
                    for (int j = 0; j < 3; j++)
                        m_matrix[i, j] *= -1;
            }

            /// <summary>
            /// Rotates current matrix at X axis
            /// </summary>
            /// <param name="angle">Angle in radians</param>
            public void RotateXAxis(double angle)
            {              
                Matrix3 RotX = new Matrix3(
                    new Vector(1, 0, 0),
                    new Vector(0, Math.Cos(angle), -Math.Sin(angle)),
                    new Vector(0, Math.Sin(angle), Math.Cos(angle)));
                
                m_matrix = (this * RotX).m_matrix;
            }
            /// <summary>
            /// Rotates current matrix at Y axis
            /// </summary>
            /// <param name="angle">Angle in radians</param>
            public void RotateYAxis(double angle)
            {

                Matrix3 RotY = new Matrix3(
                    new Vector(Math.Cos(angle), 0, Math.Sin(angle)),
                    new Vector(0, 1, 0),
                    new Vector(-Math.Sin(angle), 0, Math.Cos(angle)));

                m_matrix = (this * RotY).m_matrix;
            }
            /// <summary>
            /// Rotates current matrix at Z axis
            /// </summary>
            /// <param name="angle">Angle in radians</param>
            public void RotateZAxis(double angle)
            {
                Matrix3 RotZ = new Matrix3(
                                 new Vector(Math.Cos(angle), -Math.Sin(angle), 0),
                                 new Vector(Math.Sin(angle), Math.Cos(angle), 0),
                                 new Vector(0, 0, 1));
                
                this.Multiply(RotZ);

                //m_matrix = (this * RotZ).m_matrix;
            }
            /// <summary>
            /// Transpose this matrix
            /// </summary>
            public void Transpose()
            {
                for (int i = 0; i < 3; i++)
                {
                    for (int j = 0; j < 3; j++)
                    {
                        double temp = m_matrix[i, j];
                        m_matrix[i, j] = m_matrix[j, i];
                        m_matrix[j, i] = temp;
                    }
                }
            }
            /// <summary>
            /// Returns rotated vector by this matrix
            /// </summary>
            /// <param name="vec">vector</param>
            /// <returns></returns>
            public Vector Multiply(Vector vec)
            {
                double x, y, z;
                
                x = m_matrix[0, 0] * vec.x + m_matrix[1, 0] * vec.y + m_matrix[2, 0] * vec.z;
                y = m_matrix[0, 1] * vec.x + m_matrix[1, 1] * vec.y + m_matrix[2, 1] * vec.z;
                z = m_matrix[0, 2] * vec.x + m_matrix[1, 2] * vec.y + m_matrix[2, 2] * vec.z;
                
                return new Vector(x, y, z);
            }            
            /// <summary>
            /// Returns rotated point by this matrix
            /// </summary>
            /// <param name="vec">point</param>
            /// <returns></returns>
            public Point Multiply(Point vec)
            {
                double x, y, z;

                x = m_matrix[0, 0] * vec.x + m_matrix[1, 0] * vec.y + m_matrix[2, 0] * vec.z;
                y = m_matrix[0, 1] * vec.x + m_matrix[1, 1] * vec.y + m_matrix[2, 1] * vec.z;
                z = m_matrix[0, 2] * vec.x + m_matrix[1, 2] * vec.y + m_matrix[2, 2] * vec.z;

                return new Point(x, y, z);
            }
            
            /// <summary>
            /// Returns rotated vector by this matrix (transposed)
            /// </summary>
            /// <param name="vec">vector</param>
            /// <returns></returns>
            public Vector MultiplyByTranspose(Vector vec)
            {
                double x, y, z;

                x = m_matrix[0, 0] * vec.x + m_matrix[0, 1] * vec.y + m_matrix[0, 2] * vec.z;
                y = m_matrix[1, 0] * vec.x + m_matrix[1, 1] * vec.y + m_matrix[1, 2] * vec.z;
                z = m_matrix[2, 0] * vec.x + m_matrix[2, 1] * vec.y + m_matrix[2, 2] * vec.z;

                return new Vector(x, y, z);
            }
            /// <summary>
            /// Adds matrix to current
            /// </summary>
            /// <param name="matrix"></param>
            public void Add(Matrix3 matrix)
            {
                for (int i = 0; i < 3; i++)
                    for (int j = 0; j < 3; j++)
                    {
                        m_matrix[i, j] += matrix.m_matrix[i, j];
                    }
            }
            /// <summary>
            /// Substract matrix from current
            /// </summary>
            /// <param name="matrix"></param>
            public void Substract(Matrix3 matrix)
            {
                for (int i = 0; i < 3; i++)
                    for (int j = 0; j < 3; j++)
                    {
                        m_matrix[i, j] -= matrix.m_matrix[i, j];
                    }
            }
            /// <summary>
            /// Multiplies matrix do double value
            /// </summary>
            /// <param name="value"></param>
            public void Multiply(double value)
            {
                for (int i = 0; i < 3; i++)
                    for (int j = 0; j < 3; j++)
                    {
                        m_matrix[i, j] *= value;
                    }
            }
            /// <summary>
            /// Multiply matrix to current
            /// </summary>
            /// <param name="matrix"></param>
            public void Multiply(Matrix3 matrix)
            {
                double a, b, c, d, e, f, g, h, i;
                
                //note: temps needed so that x.multiply(x,y) works OK.
                a = m_matrix[0, 0] * matrix.m_matrix[0, 0] + m_matrix[0, 1] * matrix.m_matrix[1, 0] + m_matrix[0, 2] * matrix.m_matrix[2, 0];
                b = m_matrix[0, 0] * matrix.m_matrix[0, 1] + m_matrix[0, 1] * matrix.m_matrix[1, 1] + m_matrix[0, 2] * matrix.m_matrix[2, 1];
                c = m_matrix[0, 0] * matrix.m_matrix[0, 2] + m_matrix[0, 1] * matrix.m_matrix[1, 2] + m_matrix[0, 2] * matrix.m_matrix[2, 2];

                d = m_matrix[1, 0] * matrix.m_matrix[0, 0] + m_matrix[1, 1] * matrix.m_matrix[1, 0] + m_matrix[1, 2] * matrix.m_matrix[2, 0];
                e = m_matrix[1, 0] * matrix.m_matrix[0, 1] + m_matrix[1, 1] * matrix.m_matrix[1, 1] + m_matrix[1, 2] * matrix.m_matrix[2, 1];
                f = m_matrix[1, 0] * matrix.m_matrix[0, 2] + m_matrix[1, 1] * matrix.m_matrix[1, 2] + m_matrix[1, 2] * matrix.m_matrix[2, 2];

                g = m_matrix[2, 0] * matrix.m_matrix[0, 0] + m_matrix[2, 1] * matrix.m_matrix[1, 0] + m_matrix[2, 2] * matrix.m_matrix[2, 0];
                h = m_matrix[2, 0] * matrix.m_matrix[0, 1] + m_matrix[2, 1] * matrix.m_matrix[1, 1] + m_matrix[2, 2] * matrix.m_matrix[2, 1];
                i = m_matrix[2, 0] * matrix.m_matrix[0, 2] + m_matrix[2, 1] * matrix.m_matrix[1, 2] + m_matrix[2, 2] * matrix.m_matrix[2, 2];

                // fill data
                m_matrix[0, 0] = a;
                m_matrix[0, 1] = b;
                m_matrix[0, 2] = c;

                m_matrix[1, 0] = d;
                m_matrix[1, 1] = e;
                m_matrix[1, 2] = f;

                m_matrix[2, 0] = g;
                m_matrix[2, 1] = h;
                m_matrix[2, 2] = i;
            }

            /// <summary>
            /// 
            /// </summary>
            /// <param name="digits"></param>
            public void Round(uint digits)
            {
                double factor1 = default(double);
                double factor2 = factor1;
                if (Factor(digits, out factor1, out factor2))
                {
                    for (int i = 0; i < 3; i++)
                    {
                        for (int j = 0; j < 3; j++)
                        {
                            m_matrix[i, j] = (int)m_matrix[i, j];
                        }
                    }
                }
                else
                {
                    for (int i = 0; i < 3; i++)
                    {
                        for (int j = 0; j < 3; j++)
                        {
                            m_matrix[i, j] = ((int)(m_matrix[i, j] * factor2)) * factor1;
                        }
                    }
                }
            }

            #region operators

            /// <summary>
            /// 
            /// </summary>
            /// <param name="m1">First matrix</param>
            /// <param name="m2">Second matrix</param>
            /// <returns></returns>
            public static bool operator ==(Matrix3 m1, Matrix3 m2)
            {
                if (m1 == default(Matrix3) | m1 == default(Matrix3))
                {
                    return false;
                }
                if (m1.m_null | m2.m_null)
                    return false;       
                
                if (System.Object.ReferenceEquals(m1, m2))
                {
                    return true;
                }


                for (int i = 0; i < 3; i++)
                    for (int j = 0; j < 3; j++)
                        if (!ApproximatelyEqual(m1.m_matrix[i, j], m2.m_matrix[i, j]))
                            return false;

                return true;
            }
            public static Matrix3 operator *(Matrix3 m1, Matrix3 m2)
            {
                double a, b, c, d, e, f, g, h, i;

                //note: temps needed so that x.multiply(x,y) works OK.
                a = m1.m_matrix[0, 0] * m2.m_matrix[0, 0] + m1.m_matrix[0, 1] * m2.m_matrix[1, 0] + m1.m_matrix[0, 2] * m2.m_matrix[2, 0];
                b = m1.m_matrix[0, 0] * m2.m_matrix[0, 1] + m1.m_matrix[0, 1] * m2.m_matrix[1, 1] + m1.m_matrix[0, 2] * m2.m_matrix[2, 1];
                c = m1.m_matrix[0, 0] * m2.m_matrix[0, 2] + m1.m_matrix[0, 1] * m2.m_matrix[1, 2] + m1.m_matrix[0, 2] * m2.m_matrix[2, 2];

                d = m1.m_matrix[1, 0] * m2.m_matrix[0, 0] + m1.m_matrix[1, 1] * m2.m_matrix[1, 0] + m1.m_matrix[1, 2] * m2.m_matrix[2, 0];
                e = m1.m_matrix[1, 0] * m2.m_matrix[0, 1] + m1.m_matrix[1, 1] * m2.m_matrix[1, 1] + m1.m_matrix[1, 2] * m2.m_matrix[2, 1];
                f = m1.m_matrix[1, 0] * m2.m_matrix[0, 2] + m1.m_matrix[1, 1] * m2.m_matrix[1, 2] + m1.m_matrix[1, 2] * m2.m_matrix[2, 2];

                g = m1.m_matrix[2, 0] * m2.m_matrix[0, 0] + m1.m_matrix[2, 1] * m2.m_matrix[1, 0] + m1.m_matrix[2, 2] * m2.m_matrix[2, 0];
                h = m1.m_matrix[2, 0] * m2.m_matrix[0, 1] + m1.m_matrix[2, 1] * m2.m_matrix[1, 1] + m1.m_matrix[2, 2] * m2.m_matrix[2, 1];
                i = m1.m_matrix[2, 0] * m2.m_matrix[0, 2] + m1.m_matrix[2, 1] * m2.m_matrix[1, 2] + m1.m_matrix[2, 2] * m2.m_matrix[2, 2];

                // fill data
                Matrix3 ret = new Matrix3();
                ret.m_matrix = new double[3, 3];

                ret.m_matrix[0, 0] = a;
                ret.m_matrix[0, 1] = b;
                ret.m_matrix[0, 2] = c;

                ret.m_matrix[1, 0] = d;
                ret.m_matrix[1, 1] = e;
                ret.m_matrix[1, 2] = f;

                ret.m_matrix[2, 0] = g;
                ret.m_matrix[2, 1] = h;
                ret.m_matrix[2, 2] = i;

                return ret;
            }

            /// <summary>
            /// 
            /// </summary>
            /// <param name="m1"></param>
            /// <param name="s"></param>
            /// <returns></returns>
            public static Matrix3 operator *(Matrix3 m1, double s)
            {
                Matrix3 ret = new Matrix3();
                for (int i = 0; i < 3; i++)
                    for (int j = 0; j < 3; j++)
                    {
                        ret.m_matrix[i, j] = m1.m_matrix[i, j] * s;
                    }
                return ret;
            }
            public static Matrix3 operator -(Matrix3 m1, Matrix3 m2)
            {
                Matrix3 ret = new Matrix3();
                for (int i = 0; i < 3; i++)
                    for (int j = 0; j < 3; j++)
                    {
                        ret.m_matrix[i, j] = m1.m_matrix[i, j] - m2.m_matrix[i, j];
                    }
                return ret;
            }
            public static Matrix3 operator +(Matrix3 m1, Matrix3 m2)
            {
                Matrix3 ret = new Matrix3();
                for (int i = 0; i < 3; i++)
                    for (int j = 0; j < 3; j++)
                    {
                        ret.m_matrix[i, j] = m1.m_matrix[i, j] + m2.m_matrix[i, j];
                    }
                return ret;
            }
            public static bool operator !=(Matrix3 m1, Matrix3 m2)
            {
                return !(m1 == m2);
            }

            #endregion

            #region static memebers

            public static Matrix3 Null
            {
                get
                {
                    Matrix3 m = new Matrix3();
                    m.m_null = true;
                    m.m_matrix = new double[,]{ 
                       {0, 0, 0},
                       {0, 0, 0},
                       {0, 0, 0}
                   };
                    return m;
                }
            }
            public static Matrix3 Identity
            {
                get
                {
                    Matrix3 m = new Matrix3();
                    m.m_matrix = new double[,]{ 
                       {1, 0, 0},
                       {0, 1, 0},
                       {0, 0, 1}
                    };
                    return m;
                }
            }

            
            #endregion

            #region INullable Members

            public bool IsNull
            {
                get { return  m_matrix == null | m_null; }
            }

            #endregion

            #region IEquatable<Matrix3> Members

            public bool Equals(Matrix3 matrix)
            {                           
                return this == matrix;
            }

            public override bool Equals(object obj)
            {
                if (obj is Matrix3)
                    return base.Equals((Matrix3)obj);
                else return false;
            }            
            
            public override int GetHashCode()
            {
                return base.GetHashCode();
            }

            #endregion

            public Matrix3 Clone()
            {
                Matrix3 matrix = new Matrix3();
                matrix.m_matrix = new double[3, 3];
                matrix.Data = Data;

                return matrix;
            }
        }
        #endregion

        #region Transform definition
        //[System.Diagnostics.DebuggerDisplay("{format(8)}", Name = "Transform")]
        [System.Runtime.InteropServices.StructLayout(System.Runtime.InteropServices.LayoutKind.Sequential)]
        public struct Transform : INullable, IEquatable<Transform>
        {
            private Matrix3 rotation;
            private Vector translation;
            private double scale;
            private bool m_null;

            private string format(byte dig)
            {
                Transform t = RoundTo(this, dig);
                return string.Empty;
            }

            public Transform(double[] data): this()
            {
                if (rotation.IsNull)
                    rotation = Matrix3.Identity;

                rotation.RowMajor_set(data);
                translation = new Vector(data[9], data[10], data[11]);
                scale = data[12];
            }
            
            public double[] Data
            {
                get {
                    double[] value = new double[16];
                    //Rotation data
                    double[] rot = rotation.RowMajor_get();                    
                    for (int i = 0; i < rot.Length; i++)
                        value[i] = rot[i];
                    //Translation data
                    value[9] = translation.x;
                    value[10] = translation.y;
                    value[11] = translation.z;
                    //scale date
                    value[12] = scale; //scaling
                    
                    return value;
                }
                set
                {
                    //set Rotation data
                    rotation.RowMajor_set(value);
                    //set Translation data
                    translation.x = value[9];
                    translation.y = value[10];
                    translation.z = value[11];
                    //scale date
                    scale = value[12];
                }
            }

            public double Scale { get { return scale; } 
                //set { scale = value; } 
            }

            public Matrix3 Rotation
            {
                get { return rotation; }
                set { rotation = value; }
            }

            public Vector Translation
            {
                get { return translation; }
                set { translation = value; }
            }

            public Transform Inverse
            {
                get
                {
                    if (m_null)
                        return Null;
                   
                    Transform t = Transform.Identity;
                    t.Rotation.RowMajor_set(rotation.Inverse.RowMajor_get());
                    t.translation = rotation.Inverse.Multiply(translation * -1.0);
                    return t;
                }
            }

            public void Multiply(Transform t)
            {
                translation = t.rotation.Multiply(translation) + t.translation;
                rotation.Multiply(t.rotation);
            }

            public Point TransformPoint(Point point)
            {
                return rotation.Multiply(point) + translation;
            }

            public Vector TransformVector(Vector vector)
            {
                return rotation.Multiply(vector);
            }
            public void RotateXAxis(double angle) { rotation.RotateXAxis(angle); }
            public void RotateYAxis(double angle) { rotation.RotateYAxis(angle); }
            public void RotateZAxis(double angle) { rotation.RotateZAxis(angle); }

            #region INullable Members

            public bool IsNull
            {
                get { return m_null || rotation.IsNull || translation.IsNull; }
            }

            #endregion

            #region IEquatable<Transform> Members

            public bool Equals(Transform other)
            {
                throw new Exception("The method or operation is not implemented.");
            }

            #endregion

            #region static members
            public static Transform Null
            {
                get
                {
                    Transform t = new Transform();
                    t.rotation = Matrix3.Null;
                    t.translation = Vector.Null;
                    t.m_null = true;
                    return t;
                }
            }
            /// <summary>
            /// Returns identity transformation
            /// </summary>
            public static Transform Identity
            {
                get {
                    Transform t = new Transform();
                    t.rotation = Matrix3.Identity;
                    t.translation = new Vector();
                    t.scale = 1.0;
                    return t;
                }
            }
            #endregion

            public Transform Clone()
            {
                return new Transform(Data);
            }
        }
        #endregion

        #region Basis definition
        [System.Runtime.InteropServices.StructLayout(System.Runtime.InteropServices.LayoutKind.Sequential)]
        public struct Basis : INullable
        {
            private Point point;
            private Transform transform;
            private Plane xyPlane;
            private Plane xzPlane;
            private Plane yzPlane;
            private bool m_null;

            public Point Point { get { return point; } }
            public Vector XAxis { get { return transform.Rotation.XAxis; } }
            public Vector YAxis { get { return transform.Rotation.YAxis; } }
            public Vector ZAxis { get { return transform.Rotation.ZAxis; } }
            public Plane XYPlane { get { return xyPlane; } }
            public Plane YZPlane { get { return yzPlane; } }
            public Plane XZPlane { get { return xzPlane; } }
            public Transform Transform { get { return transform; } }

            public Basis(Point point, Vector normal)
                : this()
            {
                this.point = point;
                Vector x = normal.GetPerpVector();
                Vector y = x ^ normal;
                Vector z = normal;
                x.Inverce();
                transform = new Transform();
                transform.Rotation = new Matrix3(x, y, z);
                transform.Translation = new Point()[point];
                xyPlane = new Plane(x, y, point);
                xzPlane = new Plane(x, z, point);
                yzPlane = new Plane(y, z, point);
            }

            public Basis(Point point, Vector X, Vector Y, Vector Z)
                : this()
            {
                this.point = point;
                transform = new Transform();
                transform.Rotation = new Matrix3(X, Y, Z);
                transform.Translation = new Point()[point];
                xyPlane = new Plane(X, Y, point);
                xzPlane = new Plane(X, Z, point);
                yzPlane = new Plane(Y, Z, point);
            }

            public Basis(Point point, double alpha, double beta, double gamma)
                : this()
            {
                this.point = point;
                transform = new Transform();
                transform.Rotation = new Matrix3(alpha, beta, gamma);
                transform.Translation = new Point()[point];
                xyPlane = new Plane(transform.Rotation.XAxis, transform.Rotation.YAxis, point);
                xzPlane = new Plane(transform.Rotation.XAxis, transform.Rotation.ZAxis, point);
                yzPlane = new Plane(transform.Rotation.YAxis, transform.Rotation.ZAxis, point);
            }

            #region INullable Members

            public bool IsNull
            {
                get { return m_null | transform.IsNull; }
            }

            #endregion

            public static Basis STOCK
            {
                get
                {
                    Basis u = new Basis();
                    u.transform = new Transform();
                    Vector x = new Vector(1, 0, 0);
                    Vector y = new Vector(0, 1, 0);
                    Vector z = new Vector(0, 0, 1);
                    u.transform.Rotation = Matrix3.Identity;
                    u.xyPlane = new Plane(x, y, new Point());
                    u.xzPlane = new Plane(x, z, new Point());
                    u.yzPlane = new Plane(y, z, new Point());
                    return u;
                }
            }

            public static Basis Null
            {
                get
                {
                    Basis u = new Basis();
                    u.m_null = true;
                    u.transform = Transform.Null;
                    u.xyPlane = Plane.Null;
                    u.xzPlane = Plane.Null;
                    u.yzPlane = Plane.Null;

                    return u;
                }
            }

            public Vector ToSTOCK(Vector vector)
            {
                return vector[transform.Inverse];
            }
            public Point ToSTOCK(Point point)
            {
                return point[transform.Inverse];
            }
            public Vector ToUCS(Vector vector)
            {
                return vector[transform];
            }
            public Vector ToUCS(Vector vector)
            {
                return point[transform];
            }

            internal Basis Clone()
            {
                return new Basis(point, XAxis, YAxis, ZAxis);
            }
        }
        #endregion

        #region Beam definition
        public struct Beam : IEquatable<Beam>, INullable
        {
            /// <summary>
            /// Direction vector.
            /// </summary>
            private Vector _direction;

            /// <summary>
            /// Start beam point.
            /// </summary>
            private Point _point;

            /// <summary>
            /// 
            /// </summary>
            private bool m_null;

            #region IEquatable<Beam> Members

            /// <summary>
            /// Are two objects equal?
            /// </summary>
            /// <param name="obj">Another object</param>
            /// <returns>True - if two objects are equal to each other, else - False.</returns>
            public override bool Equals(object obj)
            {
                return base.Equals(obj);
            }

            /// <summary>
            /// Are two beams equal?
            /// </summary>
            /// <param name="point">Another beam</param>
            /// <returns>True - if two beams are equal to each other, else - False.</returns>
            public bool Equals(Beam point)
            {
                return this == point;
            }

            /// <summary>
            /// Hash code
            /// </summary>
            /// <returns></returns>
            public override int GetHashCode()
            {
                return _direction.GetHashCode() ^ _point.GetHashCode() ^ m_null.GetHashCode();
            }

            #endregion

            #region operators

            /// <summary>
            /// Compare beam and object
            /// </summary>
            /// <param name="b1">Beam</param>
            /// <param name="b2">Object</param>
            /// <returns>True - if beam equals to object, else - False.</returns>
            public static bool operator ==(Beam b1, object b2)
            {
                if (b2 == default(object))
                    return false;
                return b1 == (Beam)b2;
            }

            /// <summary>
            /// Compare beam and object
            /// </summary>
            /// <param name="b1">Beam</param>
            /// <param name="b2">Object</param>
            /// <returns>False - if beam equals to object, else - True.</returns>
            public static bool operator !=(Beam b1, object b2)
            {
                if (b2 == default(object))
                    return true;
                return b1 != (Beam)b2;
            }

            /// <summary>
            /// Compare two beams.
            /// </summary>
            /// <param name="b1">First beam</param>
            /// <param name="b2">Second beam </param>
            /// <returns>True - if they are equal, else - False.</returns>
            public static bool operator ==(Beam b1, Beam b2)
            {
                return
                   b1.Point == b2.Point
                  &&
                   b1.Direction == b2.Direction;
            }

            /// <summary>
            /// Compare two beams.
            /// </summary>
            /// <param name="b1">First beam</param>
            /// <param name="b2">Second beam </param>
            /// <returns>False - if they are equal, else - True.</returns>
            public static bool operator !=(Beam b1, Beam b2)
            {
                return !(b1 == b2);
            }
            #endregion

           
            /// <summary>
            /// Beam direction
            /// </summary>
            public Vector Direction { get { return _direction; } }

            /// <summary>
            /// Beam start point
            /// </summary>
            public Point Point { get { return _point; } }

            /// <summary>
            /// Initialize new beam
            /// </summary>
            /// <param name="point">Start point</param>
            /// <param name="direction">Beam vector direction</param>
            public Beam(Point point, Vector direction)
                : this()
            {
                m_null = false;
                _point = point;
                _direction = direction;
            }

            #region static members
            public static Beam Null
            {
                get
                {
                    Beam p = new Beam();                    
                    p._point = Point.Null;
                    p.m_null = true;
                    return p;
                }
            }

            /// <summary>
            /// Is two beams same?
            /// </summary>
            /// <param name="beam1">First beam</param>
            /// <param name="beam2">Second beam</param>
            /// <returns>True</returns>
            public static bool IsSame(Beam beam1, Beam beam2)
            {
                if (beam2.Direction.IsParallelTo(beam1.Direction))
                {
                    if (beam2.Point - beam1.Point == beam1.Direction)
                        return true;
                    if (beam1.Point - beam2.Point == beam1.Direction)
                        return true;
                }
                return true;
            }

            /// <summary>
            /// Intersection of two beams
            /// </summary>
            /// <param name="beam1">First beam</param>
            /// <param name="beam2">Second beam</param>
            /// <param name="point">Intersection point</param>
            /// <returns>true - if two beams intersect, else - False.</returns>
            public static bool Intersect(Beam beam1, Beam beam2, out Point point)
            {
                point = default(Point);
                if (!IsSame(beam1, beam2))
                {
                    if (beam1.Point == beam2.Point)
                    {
                        point = beam2.Point;
                        return true;
                    }
                    double ortLength = 0.0001;
                    Vector A = new Vector(beam1.Point.x, beam1.Point.y, beam1.Point.z);
                    Vector B = beam1.Point - beam2.Point[beam1.Direction.GetNormalized() * ortLength];

                    Vector C = new Vector(beam2.Point.x, beam2.Point.y, beam2.Point.z);
                    Vector D = beam2.Point - beam2.Point[beam2.Direction.GetNormalized() * ortLength];

                    double s;
                    if (!ApproximatelyEqual((D.x * B.y) - (D.y * B.x), 0))
                    {

                        s = (C.y * B.x) - (A.y * B.x) - (C.x * B.y) + (A.x * B.y);
                        if (ApproximatelyEqual(s, 0))
                            s = 0;

                        if (ApproximatelyEqual((D.x * B.y) - (D.y * B.x), 0))
                            s = 0;
                        else
                            s /= (D.x * B.y) - (D.y * B.x);

                        s = 1 - s;

                        C = C + (D * (1 - s));
                        point = new Point(C.x, C.y, C.z);
                    }
                    else
                        if (!ApproximatelyEqual((D.x * B.z) - (D.z * B.x), 0))
                        {
                            s = (C.z * B.x) - (A.z * B.x) - (C.x * B.z) + (A.x * B.z);
                            if (ApproximatelyEqual(s, 0))
                                s = 0;

                            if (ApproximatelyEqual((D.x * B.z) - (D.z * B.x), 0))
                                s = 0;
                            else
                                s /= (D.x * B.z) - (D.z * B.x);

                            s = 1 - s;
                            C = C + (D * (1 - s));
                            point = new Point(C.x, C.y, C.z);
                        }
                        else
                            if (!ApproximatelyEqual((D.y * B.z) - (D.z * B.y), 0))
                            {
                                s = (C.z * B.y) - (A.z * B.y) - (C.y * B.z) + (A.y * B.z);
                                if (ApproximatelyEqual(s, 0))
                                    s = 0;

                                if (ApproximatelyEqual((D.y * B.z) - (D.z * B.y), 0))
                                    s = 0;
                                else
                                    s /= (D.y * B.z) - (D.z * B.y);

                                s = 1 - s;

                                C = C + (D * (1 - s));
                                point = new Point(C.x, C.y, C.z);
                            }

                    return true;
                }
                else return false;
            }            
            #endregion

            #region INullable Members
            public bool IsNull { get { return m_null || _point.IsNull || _direction.IsNull; } }
            #endregion
        }
        #endregion

        #region Curve definition

        public struct Curve
        {
            private int _cType;
            
            public int CType
            {
                get { return _cType; }
                set { _cType = value; }
            }

        }
        #endregion

        #region Box definition
        public struct Box
        {

        }
        #endregion

        #region Set definition

        //public class Set<T> : IEnumerable
        //    where T : new()
        //{
        //    private struct link<T>: IEquatable<T>
        //    {
        //        public T Previous;
        //        public T Value;
        //        public T Next;

        //        public bool IsFirst { get { return Type.Equals(Previous, default(T)); } }
        //        public bool IsLast { get { return Type.Equals(Next, default(T)); } }
        //        public bool IsEmpty { get { return Type.Equals(Value, default(T)); } }

        //        public override bool Equals(object obj)
        //        {
        //            if (obj is T & !IsEmpty)
        //                return base.Equals((T)obj);
        //            return false;
        //        }

        //        #region IEquatable<T> Members

        //        public bool Equals(T other)
        //        {
        //            if (!IsEmpty)
        //                return Value.Equals(other);
        //            return false;
        //        }

        //        #endregion
        //    }

        //    public int IndexOf(T item) 
        //    {
        //        return -1;
        //    }

        //    public int IndexOf(object item) {
        //        if (item is T)
        //            return IndexOf((T)item);
        //        return -1;
        //    }
        //}


        //public class Set<T> : IEnumerable
        //    where T : new()
        //{
        //    private T[] values = new T[0];
        //    private int maxIndex = 0;

        //    #region static members
        //    static int indexOf(ref Set<T> set, T value)
        //    {
        //        for (int i = 0; i < set.values.Length; i++)
        //        {
        //            if (set.values[i].Equals(value))
        //                return i;
        //        }
        //        return -1;
        //    }
        //    public static Set<T> operator -(Set<T> set, T value)
        //    {
        //        int index = indexOf(ref set, value);
        //        if (index > -1)
        //        {
        //            int idx = 0;
        //            Set<T> temp = new Set<T>();
        //            temp.values = new T[temp.maxIndex = set.values.Length - 1];
        //            for (int i = 0; i < set.values.Length; i++)
        //            {
        //                if (i == index)
        //                    continue;
        //                idx++;
        //                temp.values[idx] = set.values[i];
        //            }

        //            return temp;
        //        }
        //        return set;
        //    }
        //    public static Set<T> operator +(Set<T> set, T value)
        //    {
        //        int index = indexOf(ref set, value);
        //        if (index < 0)
        //        {
        //            Set<T> temp = new Set<T>();
        //            temp.values = new T[temp.maxIndex = set.values.Length + 1];
        //            for (int i = 0; i < set.values.Length; i++)
        //            {
        //                temp.values[i] = set.values[i];
        //            }
        //            temp.values[temp.values.Length - 1] = value;
        //            return temp;
        //        }
        //        else ;
        //        return set;
        //    }
        //    public static Set<T> operator +(Set<T> set, Set<T> values)
        //    {
        //        for (int i = 0; i < values.Length; i++)
        //        {
        //            set += values[i];
        //        }
        //        return set;
        //    }

        //    public static bool operator ==(Set<T> set, Set<T> values)
        //    {
        //        return !(set != values);
        //    }

        //    public static bool operator !=(Set<T> set, Set<T> values)
        //    {
        //        if (set.values.Length != values.values.Length)
        //            return true;

        //        for (int i = 0; i < set.Length; i++)
        //            if (!set.values[i].Equals(values[i]))
        //                return true;
        //        return false;
        //    }

        //    public static bool operator ==(Set<T> set, T[] values)
        //    {
        //        return !(set != values);
        //    }

        //    public static bool operator !=(Set<T> set, T[] values)
        //    {
        //        if (set.values.Length != values.Length)
        //            return true;

        //        for (int i = 0; i < set.Length; i++)
        //            if (!set.values[i].Equals(values[i]))
        //                return true;
        //        return false;
        //    }
        //    #endregion

        //    public Set()
        //    {

        //    }
        //    public Set(T[] values)
        //    {
        //        this.values = values;
        //    }

        //    public T this[int index]
        //    {
        //        get { return values[index]; }
        //        set { values[index] = value; }
        //    }

        //    public int Length { get { return maxIndex; } }

        //    #region IEnumerable<T> Members

        //    public IEnumerator GetEnumerator()
        //    {
        //        return values.GetEnumerator();
        //    }

        //    #endregion
        //}
        #endregion

    }
}
